diff --git a/.gitignore b/.gitignore index 9c575857..9df32a80 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,6 @@ __pycache__ runs outputs -/artifacts/ .pdm-python logs .idea diff --git a/AGENTS.md b/AGENTS.md index 0c29214e..4524723f 100644 --- a/AGENTS.md +++ b/AGENTS.md @@ -61,6 +61,16 @@ Review 或修改 initializer 时: - 检查仓库使用方、文档、环境/插件注册和 import 环风险; - 清理后验证 package import 与注册行为。 +### Package README + +README 从宏观上说明 package:它提供什么能力、如何安装与使用、以及该 package 特有的契约与边界。不要把 README +写成实现文档——内部数值(增益、维度、阈值列表)、actor 数学公式、逐字段契约说明、调度器/时钟语义等应留在代码、 +设计文档(`wiki/`)或用户教程(`docs/`)中,README 最多以一两句话概括并链接过去。不要叙述修改历史、调试过程或 +对旧实现/历史报告作辩护。 + +框架级契约(Runtime/ControlSession/Controller 语义、rollout result 口径等)只在一处维护——对应框架 package 的 +README 或教程;插件与任务 package 的 README 不重复这些内容,只描述本 package 特有的部分。 + ### Manager Kernel 入口方法 Manager fused-kernel 入口方法必须用 `@dispatch` 装饰,绝不使用 `@njit`——Numba 编译、缓存和内联由 manager diff --git a/artifacts/g1-wbt-dance.deploy/manifest.json b/artifacts/g1-wbt-dance.deploy/manifest.json new file mode 100644 index 00000000..01ce7d53 --- /dev/null +++ b/artifacts/g1-wbt-dance.deploy/manifest.json @@ -0,0 +1,371 @@ +{ + "control": { + "angular_velocity_frame": "body", + "base_orientation": "body_to_world", + "mode": "joint_servo", + "period_s": 0.02, + "quaternion_order": "xyzw", + "state_timeout_s": 0.1 + }, + "payloads": [ + { + "path": "payloads/motion.npz", + "sha256": "5dec09a5d1b0e37f98ea05002eb95194750422254d2bb0558a01a25a9387725a" + } + ], + "policy": { + "component_version": "onnx/v1", + "input": { + "dtype": "float32", + "name": "obs", + "shape": [ + 1, + 154 + ] + }, + "output": { + "dtype": "float32", + "name": "actions", + "shape": [ + 1, + 29 + ] + }, + "payload_path": "policy/model.onnx", + "sha256": "47d53c5e0126c7891765ecc7c962572a4eccffc939427bf7e8be65f475f156ba" + }, + "robot": { + "base_link_name": "pelvis", + "default_joint_position": [ + -0.31200000643730164, + 0.0, + 0.0, + 0.6690000295639038, + -0.3630000054836273, + 0.0, + -0.31200000643730164, + 0.0, + 0.0, + 0.6690000295639038, + -0.3630000054836273, + 0.0, + 0.0, + 0.0, + 0.0, + 0.20000000298023224, + 0.20000000298023224, + 0.0, + 0.6000000238418579, + 0.0, + 0.0, + 0.0, + 0.20000000298023224, + -0.20000000298023224, + 0.0, + 0.6000000238418579, + 0.0, + 0.0, + 0.0 + ], + "joint_names": [ + "left_hip_pitch_joint", + "left_hip_roll_joint", + "left_hip_yaw_joint", + "left_knee_joint", + "left_ankle_pitch_joint", + "left_ankle_roll_joint", + "right_hip_pitch_joint", + "right_hip_roll_joint", + "right_hip_yaw_joint", + "right_knee_joint", + "right_ankle_pitch_joint", + "right_ankle_roll_joint", + "waist_yaw_joint", + "waist_roll_joint", + "waist_pitch_joint", + "left_shoulder_pitch_joint", + "left_shoulder_roll_joint", + "left_shoulder_yaw_joint", + "left_elbow_joint", + "left_wrist_roll_joint", + "left_wrist_pitch_joint", + "left_wrist_yaw_joint", + "right_shoulder_pitch_joint", + "right_shoulder_roll_joint", + "right_shoulder_yaw_joint", + "right_elbow_joint", + "right_wrist_roll_joint", + "right_wrist_pitch_joint", + "right_wrist_yaw_joint" + ], + "position_lower": [ + -2.5306999683380127, + -0.5235998630523682, + -2.7576000690460205, + -0.08726692199707031, + -0.87267005443573, + -0.26179999113082886, + -2.5306999683380127, + -2.967099905014038, + -2.7576000690460205, + -0.08726692199707031, + -0.87267005443573, + -0.26179999113082886, + -2.618000030517578, + -0.5199999809265137, + -0.5199999809265137, + -3.089200019836426, + -1.5881998538970947, + -2.618000030517578, + -1.0471999645233154, + -1.9722199440002441, + -1.6144299507141113, + -1.6144299507141113, + -3.089200019836426, + -2.251499891281128, + -2.618000030517578, + -1.0471999645233154, + -1.9722199440002441, + -1.6144299507141113, + -1.6144299507141113 + ], + "position_upper": [ + 2.8798000812530518, + 2.967099905014038, + 2.7576000690460205, + 2.8798000812530518, + 0.5235999822616577, + 0.26179999113082886, + 2.8798000812530518, + 0.5235998630523682, + 2.7576000690460205, + 2.8798000812530518, + 0.5235999822616577, + 0.26179999113082886, + 2.618000030517578, + 0.5199999809265137, + 0.5199999809265137, + 2.6703996658325195, + 2.251499891281128, + 2.618000030517578, + 2.094399929046631, + 1.9722199440002441, + 1.6144299507141113, + 1.6144299507141113, + 2.6703996658325195, + 1.5881998538970947, + 2.618000030517578, + 2.094399929046631, + 1.9722199440002441, + 1.6144299507141113, + 1.6144299507141113 + ], + "torque_limit": [ + 88.0, + 139.0, + 88.0, + 139.0, + 50.0, + 50.0, + 88.0, + 139.0, + 88.0, + 139.0, + 50.0, + 50.0, + 88.0, + 50.0, + 50.0, + 25.0, + 25.0, + 25.0, + 25.0, + 25.0, + 5.0, + 5.0, + 25.0, + 25.0, + 25.0, + 25.0, + 25.0, + 5.0, + 5.0 + ] + }, + "schema_version": "motrix-deploy/v1", + "source": { + "checkpoint": "checkpoints/latest.pt", + "framework": "motrix.fastsac/torch", + "run_id": "26-10-07_21-28-43-758744" + }, + "task": { + "config": { + "action_scale": [ + 0.5475465059280396, + 0.3506614565849304, + 0.5475465059280396, + 0.3506614565849304, + 0.4385773241519928, + 0.4385773241519928, + 0.5475465059280396, + 0.3506614565849304, + 0.5475465059280396, + 0.3506614565849304, + 0.4385773241519928, + 0.4385773241519928, + 0.5475465059280396, + 0.4385773241519928, + 0.4385773241519928, + 0.4385773241519928, + 0.4385773241519928, + 0.4385773241519928, + 0.4385773241519928, + 0.4385773241519928, + 0.07450086623430252, + 0.07450086623430252, + 0.4385773241519928, + 0.4385773241519928, + 0.4385773241519928, + 0.4385773241519928, + 0.4385773241519928, + 0.07450086623430252, + 0.07450086623430252 + ], + "kd": [ + 2.557889699935913, + 6.308801651000977, + 2.557889699935913, + 6.308801651000977, + 1.8144457340240479, + 1.8144457340240479, + 2.557889699935913, + 6.308801651000977, + 2.557889699935913, + 6.308801651000977, + 1.8144457340240479, + 1.8144457340240479, + 2.557889699935913, + 1.8144457340240479, + 1.8144457340240479, + 0.9072228670120239, + 0.9072228670120239, + 0.9072228670120239, + 0.9072228670120239, + 0.9072228670120239, + 1.0681414604187012, + 1.0681414604187012, + 0.9072228670120239, + 0.9072228670120239, + 0.9072228670120239, + 0.9072228670120239, + 0.9072228670120239, + 1.0681414604187012, + 1.0681414604187012 + ], + "kp": [ + 40.179237365722656, + 99.09842681884766, + 40.179237365722656, + 99.09842681884766, + 28.501245498657227, + 28.501245498657227, + 40.179237365722656, + 99.09842681884766, + 40.179237365722656, + 99.09842681884766, + 28.501245498657227, + 28.501245498657227, + 40.179237365722656, + 28.501245498657227, + 28.501245498657227, + 14.250622749328613, + 14.250622749328613, + 14.250622749328613, + 14.250622749328613, + 14.250622749328613, + 16.77832794189453, + 16.77832794189453, + 14.250622749328613, + 14.250622749328613, + 14.250622749328613, + 14.250622749328613, + 14.250622749328613, + 16.77832794189453, + 16.77832794189453 + ], + "motion_frames": 999, + "period_s": 0.02, + "preparation_joint_position": null, + "preparation_kd": [ + 5.0, + 5.0, + 5.0, + 10.0, + 5.0, + 5.0, + 5.0, + 5.0, + 5.0, + 10.0, + 5.0, + 5.0, + 5.0, + 5.0, + 5.0, + 3.0, + 3.0, + 3.0, + 3.0, + 3.0, + 3.0, + 3.0, + 3.0, + 3.0, + 3.0, + 3.0, + 3.0, + 3.0, + 3.0 + ], + "preparation_kp": [ + 350.0, + 200.0, + 200.0, + 300.0, + 300.0, + 150.0, + 350.0, + 200.0, + 200.0, + 300.0, + 300.0, + 150.0, + 200.0, + 200.0, + 200.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0, + 40.0 + ], + "preparation_ramp_duration_s": 2.0, + "preparation_settle_duration_s": 1.0, + "preparation_timeout_s": 5.0, + "reference_body_name": "torso_link", + "termination_joint_position_threshold": 0.5, + "termination_joint_velocity_threshold": 100.0, + "termination_ref_orientation_threshold": 0.8 + }, + "name": "g1_wbt/v1" + } +} diff --git a/artifacts/g1-wbt-dance.deploy/payloads/motion.npz b/artifacts/g1-wbt-dance.deploy/payloads/motion.npz new file mode 100644 index 00000000..bd737ef2 --- /dev/null +++ b/artifacts/g1-wbt-dance.deploy/payloads/motion.npz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5dec09a5d1b0e37f98ea05002eb95194750422254d2bb0558a01a25a9387725a +size 496308 diff --git a/artifacts/g1-wbt-dance.deploy/policy/model.onnx b/artifacts/g1-wbt-dance.deploy/policy/model.onnx new file mode 100644 index 00000000..66c477af Binary files /dev/null and b/artifacts/g1-wbt-dance.deploy/policy/model.onnx differ diff --git a/docs/source/en/user_guide/envs/index.md b/docs/source/en/user_guide/envs/index.md index 5c9887cf..9885bbc4 100644 --- a/docs/source/en/user_guide/envs/index.md +++ b/docs/source/en/user_guide/envs/index.md @@ -58,7 +58,8 @@ python scripts/view.py env= | g1-29dof-wbt-largebox | `g1-29dof-wbt-largebox` | Track a large-box carrying reference motion with Unitree G1. | `motrix.fastsac` | | g1-walk-flat | `g1-walk-flat` | Track walking commands with Unitree G1 on flat ground. | `motrix.fastsac` | | g1-walk-rough | `g1-walk-rough` | Track walking commands with Unitree G1 over uneven terrain. | `motrix.fastsac` | -| g1-wbt-dance | `g1-wbt-dance` | Track the bundled G1 dance motion with the manager-based environment. | `motrix.fastsac` | +| g1-wbt-backflip | `g1-wbt-backflip` | Track the bundled G1 backflip reference motion. | `motrix.fastsac` | +| g1-wbt-dance | `g1-wbt-dance` | Track the G1 dance motion corpus with the manager-based environment. | `motrix.fastsac` | | go1-stairs-terrain-walk | `go1-stairs-terrain-walk` | Control Unitree Go1 to walk over stair terrain. | `rslrl.ppo`, `skrl.ppo` | | go1-walk-flat | `go1-walk-flat` | Track walking commands with Unitree Go1 on flat ground. | `rslrl.ppo`, `skrl.ppo` | | go1-walk-rough | `go1-walk-rough` | Track walking commands with Unitree Go1 on a procedural rough height field. | `rslrl.ppo`, `skrl.ppo` | 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 cff16bf1..0d2f36d8 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,9 +29,20 @@ input_devices_and_bindings/extending Backend-neutral InputDevice, binding, and task-command pipeline ``` -`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. +Policy means observation → action: `Policy` runs the model. Controller means robot +state plus control input → `RobotCommand`. `DeployTask` is the complete task Controller; +the execution hierarchy is Runtime → ControlSession → DeployTask → phase controllers. + +ControlSession reads an optional binding (or uses `None` for autonomous operation), constructs +`ControlContext`, then calls `task.step(state, context)`. The task owns preparation, model +execution and normal-stop behavior as required by that task. Its `PolicyController` uses a +`PolicyProcessor` adapter for observation processing, inference and action conversion, without +robot/backend handles or a reference back to the complete task. ControlSession writes +`ControllerStep.command` and owns open/enable, state validation, health checks, cleanup +and statistics. Runtime owns session scheduling and host lifecycle; only SimulationRuntime +owns physics/viewer and simulation world resources. Hardware uses the same control path +against live state; neither ControlSession nor Runtime interprets task phases. Faults bypass +normal task stopping and invoke the backend fallback immediately. 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. @@ -54,29 +65,12 @@ A high-level task command also has the opposite direction from `RobotCommand`: ## How a Binding Reaches the Task -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, - elapsed_time_s=elapsed_time_s, - command=command, -) -task.validate_command(context.command) -observation = task.build_observation(state, context) -``` - -Deployment's robot I/O boundary `RobotInterface` and optional `KeyboardDeviceProvider` / `GamePadDeviceProvider` -capability protocols are defined in `motrix_deploy.robot.interface`; import them from that module. The providers expose -input devices whose lifecycle they own, rather than discovering runtime plugins. Runtime discovery remains in -`motrix_deploy.runtime.factory` through `motrix_deploy.backends`, and recipes still select plugins with `runtime.backend`. -The prepared `runtime.robot` is available before opening. The application constructs -`ControlSession(robot=runtime.robot, command_binding=binding, ...)` explicitly, -binds it with `runtime.bind_control_session(control)`, and runs it inside `with runtime`. -The factory does not construct bindings or controllers through callbacks. +In a training environment, the environment reads one command batch through the binding before +each step and hands it to the task to build observations. In deployment, the `ControlSession` +holds the same optional `CommandBinding` (`None` for autonomous tasks) and passes the read +command to the task inside `ControlContext.command` — both sides consume the same typed +command. See the {doc}`deployment tutorial ` for the complete session and +runtime assembly. 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 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 96f74b0b..20d2824b 100644 --- a/docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md +++ b/docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md @@ -32,14 +32,22 @@ Export selects the latest run and creates `artifacts/go2-walk-flat.deploy/`. To `run=` instead of `env`; `output=` changes the destination. Existing artifact directories are not overwritten. Confirm that inspection reports `valid: true`. -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` -or `motrixsim` for simulation, or `unitree_go2` for hardware). `runtime.backend` is a scalar string selector, not a nested configuration -mapping or Hydra group. +The artifact directory is self-contained: + +```text +go2-walk-flat.deploy/ +├── manifest.json # robot/control/task settings, payload checksums +├── policy/model.onnx # exported actor with input normalization +└── task/ # task-owned payloads (e.g. the G1 motion NPZ) +``` + +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` 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 @@ -67,10 +75,10 @@ The complete `examples/deploy_to_sim.py` example composes a scene, walking task, python examples/deploy_to_sim.py --headless --steps 100 ``` -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. +The example loads the model externally and injects it into the task; the task computes commands, the control session +owns robot I/O and execution statistics, and the runtime owns physics, scheduling and the viewer. The +[advanced assembly](#assemble-the-runtime-and-task-explicitly) section shows the same composition step by step. +Raw ONNX use requires explicit task preprocessing and action settings; the artifact supplies those settings for the CLI. ### Select native MotrixSim @@ -123,13 +131,44 @@ motrix-deploy-unitree joint-control enp5s0 FL_thigh_joint 0.9 \ This helper uses the artifact's robot, gain, timing, and limit settings. It waits for Start and A, moves the joint, holds, returns to the default pose, and closes with damping. -## Advanced usage +## Deploy the G1 WBT dance + +The G1 whole-body-tracking dance task uses the same export → artifact → `motrix-deploy` pipeline. The artifact embeds +the servo gains, action scales, preparation gains and timing, safety thresholds, and the motion clip itself (as a +`payloads/motion.npz` payload with its checksum in the manifest), so deployment needs neither Torch nor an RL environment. +Export needs the training provider and task environment packages, but does **not** train: + +```bash +source .venv/bin/activate +RUN=runs/g1-wbt-dance/motrix/torch/fastsac/ +python scripts/export_deploy.py run="$RUN" \ + output=artifacts/g1-wbt-dance-software-pd.deploy validation.atol=3e-5 + +motrix-deploy task=g1-wbt-dance/sim artifact=artifacts/g1-wbt-dance.deploy +``` + +Headless validation and artifact inspection: + +```bash +motrix-deploy task=g1-wbt-dance/sim artifact=artifacts/g1-wbt-dance.deploy \ + runtime.viewer=false +motrix-deploy inspect artifact=artifacts/g1-wbt-dance.deploy +``` + +The default duration comes from the embedded motion clip; shorter runs use `duration_s`, longer durations are rejected +before opening the backend instead of looping or holding. No velocity command is needed: the motion is artifact-owned. +The same artifact runs on MuJoCo and MotrixSim (`runtime.backend=motrixsim`, with +`runtime.physics.solver_iterations=3` matching the training budget). It is not a hardware deployment recipe: the current +Unitree transport is Go2-specific, and sim2sim success is not evidence of hardware safety. -Artifact inspection resolves the versioned task class through `motrix_deploy.tasks` and validates its Pydantic -`spec_type`, without constructing a task or importing training environments or simulator backends. -The JSON wire format remains `{name, config}`. Python uses direct spec attributes such as `spec.kp` and -`spec.action_scale`; action scale and bounds are joint vectors in canonical order. Policy action bounds apply -before scaling and adding the default pose; robot position limits apply to the resulting targets. +The task mirrors real deployment practice with four phases: `prepare` (ramp measured joints to the default standing +pose, gated on continuously measured readiness), `policy_hold`, `playback`, and `damping`. Takeover after readiness is +automatic by default; programmatic applications can stage it manually with `automatic_start=False`, the request +methods, or an attached keyboard (`p` start policy, `m` start motion, `o` stop). Note that the dance ends in a +deliberately leaned pose that only the tracking policy can balance: after the playback budget the task enters measured +damping directly, and ending upright requires the motion clip itself to finish at the standing pose. + +## Advanced usage ### Load a task spec from YAML @@ -148,40 +187,17 @@ spec = Go2WalkTaskSpec.model_validate(OmegaConf.to_container(cfg, resolve=True)) `Go2WalkTaskSpec` uses field constraints and a model validator for joint-vector lengths and ordered bounds. No separate `validate()` call is needed. Use the artifact reader for a full deployment manifest. -### Startup and array validation - -`RobotInterface.enable()` takes no initial command. Unitree owns the transition to -`RobotSpec.default_joint_position`, using zero target velocity/feedforward torque -and the resolved startup gains. `runtime.kp` / `runtime.kd` override artifact gains; -`null` retains the artifact gains. Startup does not call task `process_action()` -with a zero action. Initial-state safety and task-termination checks still run before -enable, and fresh-state checks run afterward; Start/A gating remains unchanged. - -`DeployTask` declares neither `observation_size` nor `action_size`. The policy -input boundary validates actual observations, and the task action-conversion -boundary validates actual policy outputs. CLI assembly does not precheck task -dimensions. Manifest tensor specs remain model metadata; `DeploymentProfile` -dimensions remain export-only checks against the compiled environment. - ### Hardware sensor and motor wiring `motrix_deploy_unitree.hardware.UnitreeGo2HardwareCfg` owns asset-free hardware poses and wiring. `HardwareSensorBinding` maps SDK IMU fields, including `wxyz` quaternion conversion to canonical `xyzw`; -`UnitreeMotorBinding` maps canonical joint names to SDK motor indices. These definitions live in the -hardware plugin's `sensor` and `actuation` modules, not in simulation robot models. -The artifact still owns the policy joint order, default pose, and limits. Hardware configuration imports -neither model assets nor a simulator or SDK eagerly, and does not fabricate base position or linear velocity. +`UnitreeMotorBinding` maps canonical joint names to SDK motor indices. The artifact still owns the policy joint +order, default pose, and limits. ### Select a deployment world -Select a world with `runtime.deploy_env_id` and a robot with the existing registry ID `runtime.robot_id`. -The CLI calls `motrix_deploy.env.assemble_deploy_scene(deploy_env_id, robot_id)`: it creates the robot-free world, -fills `scene.objs.robot` using `motrix_env_core.registry.make_robot_config(robot_id)`. -The core registry lazily discovers installed `motrix_env_core.robots` entry points, shared by training and deployment. -The deployment worlds `flat` and `rough` live in independent `motrix_deploy_tasks.envs` modules. They use core -floor/height-field configuration without importing a training task, robot model, or training contact setup. - -To keep the same Go2 recipe and artifact but switch its world: +Select a world with `runtime.deploy_env_id` and a robot with `runtime.robot_id`. To keep the same Go2 recipe and +artifact but switch its world: ```bash motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy \ @@ -190,6 +206,9 @@ motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy \ A world-only override leaves robot attachment placement unchanged. Adjust `runtime.robot_translation` if the terrain needs different clearance; the complete `task=go2-walk-rough/sim` recipe supplies its own attachment defaults. +`RobotCfg.translation`/`rotation` are attachment transforms composed once with the model's intrinsic base pose: +Go2's intrinsic base height is 0.445 m, so the flat recipe's -0.114 m offset gives 0.331 m and the rough recipe's +-0.025 m offset gives 0.42 m. The installed flat recipe selects the world and robot by ID: @@ -203,34 +222,15 @@ runtime: robot_rotation: [0.0, 0.0, 0.0, 1.0] ``` -`runtime.physics` and `runtime.sensor_bindings` remain separate settings. MuJoCo accepts only the completed `SceneCfg`; -it neither selects an environment/robot pair nor assembles the scene or queries a training registry. -Programmatic `SimulationRuntimeConfig.scene` receives the same complete scene with the robot declared only in `scene.objs.robot`. - -Placement has one source: `RobotCfg.translation` (Vec3) and `rotation` (xyzw) are attachment transforms that compose once -with the model's intrinsic base pose, not absolute world root poses. Go2's intrinsic base height is 0.445 m; the flat -recipe's -0.114 m offset gives 0.331 m, and the rough recipe's -0.025 m offset gives 0.42 m with identity rotation. -The runtime preserves these transforms on its copied scene. `mj_resetData` restores the compiled `model.qpos0`, including -the composed root placement; initialization overrides only articulation joints with the bound `RobotSpec` key pose. - -For an external world plugin, declare a zero-argument `SceneCfg` factory in the `motrix_deploy.envs` entry-point group. -`motrix_deploy.env.available_deploy_envs()` lists installed IDs without loading factories; -`create_deploy_env(id)` loads only the selected factory and requires `scene.objs.robot` to be empty. -The core assembly helper inserts the robot selected by the existing robot registry; no generic scene-factory registry -or Hydra scene instantiation is involved. The CLI consumes `deploy_env_id`, `robot_id`, and optional -`robot_translation` / `robot_rotation`, applying attachment overrides to `scene.objs.robot.translation` / `.rotation` -before passing the ready scene to the simulation plugin. These application options are not runtime backend fields. - -`SimulationRuntimeConfig` owns scene, rendering, realtime pacing, and state-source settings. -Its `physics` field uses the shared `motrix_env_core.config.sim.SimCfg` for `dt`, -`solver_iterations`, `solver_tolerance`, and `gravity`, defaulting to -`SimCfg(dt=0.002, solver_iterations=100)`. Both backend compilers consume this field directly: -optional solver/gravity fields set to `None` preserve model-source values; explicit values override them. -The YAML layout remains `runtime.physics`, independent of scene and state sensors. -Set physics values before runtime construction: the runtime reads the shared `SimCfg` used for -compilation; changing `dt` after compilation is not a supported runtime retuning workflow. - -Programmatic applications use the same helper and pass its result to `SimulationRuntimeConfig`: +`runtime.physics` uses the shared `SimCfg` (`dt`, `solver_iterations`, `solver_tolerance`, `gravity`, defaulting to +`dt=0.002, solver_iterations=100`); optional fields set to `None` preserve model-source values. Set physics values +before runtime construction — changing `dt` after compilation is not supported. + +`runtime.sensor_bindings` maps state roles to model-local sensor names and supports only `base_angular_velocity`, +`base_linear_acceleration`, and `base_linear_velocity` (`str | None`). MuJoCo requires the angular-velocity and +acceleration bindings; the optional `base_linear_velocity` remains `None` when unbound. + +Programmatic applications assemble the same scene and pass it to `SimulationRuntimeConfig`: ```python from motrix_deploy.env import assemble_deploy_scene @@ -251,54 +251,44 @@ config = SimulationRuntimeConfig( ) ``` -`SimulationRuntimeConfig.sensor_bindings` uses the frozen `SensorBindings` dataclass, -not a Python dictionary. It supports only `base_angular_velocity`, -`base_linear_acceleration`, and `base_linear_velocity`, each `str | None` with a -`None` default. Names must be nonempty and contain a nonwhitespace character; -unknown roles and non-string values are rejected. YAML `runtime.sensor_bindings` -keeps the same mapping keys and is converted with `SensorBindings(**mapping)` -at the configuration boundary. An absent field or explicit `None` means no binding; -MuJoCo rejects either for its three required roles when opening the robot. -The backend still checks sensor existence, type, dimension, and frame; it does not -synthesize state for other roles or bind every `RobotState` field to a sensor. +Additional worlds come from installed plugins: declare a zero-argument `SceneCfg` factory in the +`motrix_deploy.envs` entry-point group, and `motrix_deploy.env.available_deploy_envs()` lists the installed IDs. -### Assemble the runtime and controller explicitly +### Assemble the runtime and task explicitly -The runtime factory selects the backend and prepares `runtime.robot`; its bound -`RobotSpec` is available before `open()`. The application creates the task, policy, -and command binding, then constructs and binds the control session. Using the -`config` above and your application-owned `task`, `policy`, and `command_binding`: +The runtime factory selects the backend and prepares `runtime.robot`; its `RobotSpec` is available before `open()`. +Load the model externally, then construct and bind the session. Using the `config` above and your application-owned +`task_spec`, `model`, and Go2 `command_binding`: ```python from motrix_deploy.runtime.control import ControlSession from motrix_deploy.runtime.factory import create_simulation_runtime +from motrix_deploy.task import create_task runtime = create_simulation_runtime("mujoco", config) robot = runtime.robot -control = ControlSession( +task = create_task(task_spec, robot.spec, model, steps=50) +session = ControlSession( robot=robot, - task=task, - policy=policy, + controller=task, command_binding=command_binding, period_s=0.02, + state_timeout_s=0.1, ) -runtime.bind_control_session(control) +runtime.bind_session(session) with runtime: - result = runtime.run(steps=50) + result = runtime.run() ``` -Construct a robot-dependent task from `robot.spec` after runtime creation. Simulation -runtimes derive `RobotSpec` only from the scene; they do not accept an external spec. -For artifact deployment, the CLI validates the artifact's robot contract against -`runtime.robot.spec` before assembling control. -`ControlSession.period_s` is the single control-period setting; `SimulationRuntimeConfig` -does not duplicate it. The simulation runtime validates it against the physics timestep -when binding or resolving the session. Runtime creation does not accept controller-building callbacks. The runtime context owns -world resources and simulation advancement; `ControlSession` owns shared control -execution, and `RobotInterface` owns robot I/O without advancing or closing the world. -Hardware applications use `create_hardware_runtime(name, config, context)` followed -by the same explicit construction and binding. Direct callers can also supply a -session to `runtime.run(control, steps=...)`. +`create_task(spec, robot, policy, steps=None)` resolves the concrete task class from the spec's task name; tasks that +reference artifact payloads (such as the G1 motion NPZ) receive the artifact through the same call. The control period +must be an integer multiple of the physics timestep. `command_binding` defaults to `None`: autonomous tasks like the +G1 dance omit it, while Go2 walking requires an externally assembled velocity binding whose values the task validates +against its velocity bounds. Hardware applications use `create_hardware_runtime(name, config, context)` and the same +session assembly. + +`runtime.run()` returns a rollout result with the success flag, exit reason, completed model steps, wall time, and +latency statistics; simulation runs additionally report `real_time_factor`. ### Select native MotrixSim deployment @@ -312,19 +302,16 @@ motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy \ The same override works for `task=go2-walk-rough/sim`; no duplicated task recipe is needed. Programmatic applications call `create_simulation_runtime("motrixsim", config)` -and follow the explicit session-binding sequence above. The Go2 state mappings remain +and follow the explicit session-assembly sequence above. The Go2 state mappings remain `gyro`, `accelerometer`, and `global_linvel`: native sensors validate local angular -velocity/acceleration and world-frame linear velocity. The runtime owns native MSD -compilation, model/data, reset, physics, and optional SDK `RenderApp` rendering, -not a MuJoCo runtime or training environment. Removing `runtime.viewer=false` enables the +velocity/acceleration and world-frame linear velocity. Removing `runtime.viewer=false` enables the native viewer and its keyboard provider in a working graphical environment. For the programmatic ONNX example, run `python examples/deploy_to_sim.py --backend motrixsim --headless --steps 50` with a matching ONNX policy; see the example's policy-file options. This path loads raw ONNX, not a `.deploy` manifest: task preprocessing, tensor specs, and control settings are explicit -in the Python example and must match the model. Its scene and controller are shared -with the default MuJoCo path. Headless uses a zero-velocity command; windowed mode uses +in the Python example and must match the model. Headless uses a zero-velocity command; windowed mode uses the viewer keyboard provider when available. ### Use an external Hydra config diff --git a/docs/source/zh_CN/user_guide/envs/index.md b/docs/source/zh_CN/user_guide/envs/index.md index 65ed6f0f..146fc909 100644 --- a/docs/source/zh_CN/user_guide/envs/index.md +++ b/docs/source/zh_CN/user_guide/envs/index.md @@ -58,7 +58,8 @@ python scripts/view.py env= | g1-29dof-wbt-largebox | `g1-29dof-wbt-largebox` | 让 Unitree G1 跟踪搬运大箱子的参考动作。 | `motrix.fastsac` | | g1-walk-flat | `g1-walk-flat` | 控制 Unitree G1 在平地上跟踪行走指令。 | `motrix.fastsac` | | g1-walk-rough | `g1-walk-rough` | 控制 Unitree G1 在起伏地形上跟踪行走指令。 | `motrix.fastsac` | -| g1-wbt-dance | `g1-wbt-dance` | 让 Unitree G1 跟踪内置舞蹈参考动作。 | `motrix.fastsac` | +| g1-wbt-backflip | `g1-wbt-backflip` | 让 Unitree G1 跟踪内置后空翻参考动作。 | `motrix.fastsac` | +| g1-wbt-dance | `g1-wbt-dance` | 让 Unitree G1 在 dance 语料目录上做全身跟踪;目录内一个 clip | `motrix.fastsac` | | go1-stairs-terrain-walk | `go1-stairs-terrain-walk` | 控制 Unitree Go1 在台阶地形上行走。 | `rslrl.ppo`, `skrl.ppo` | | go1-walk-flat | `go1-walk-flat` | 控制 Unitree Go1 在平地上跟踪行走指令。 | `rslrl.ppo`, `skrl.ppo` | | go1-walk-rough | `go1-walk-rough` | 控制 Unitree Go1 在程序化粗糙高度场上跟踪行走指令。 | `rslrl.ppo`, `skrl.ppo` | 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 615fac54..52b230b3 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,9 +27,16 @@ input_devices_and_bindings/extending InputDevice、Binding 与 backend-neutral task command 控制链路 ``` -`ControlSession` 组合 binding、任务、策略和 `RobotInterface`,读取指令、构造任务观察、运行策略,并写入 -`RobotCommand`。仿真运行时推进物理并管理 viewer;硬件运行时根据实时机器人状态调度控制。 -两者使用相同的任务指令语义。 +Policy 表示 observation → action:`Policy` 执行模型。Controller 表示机器人状态与控制输入 +→ `RobotCommand`。`DeployTask` 是完整任务 Controller,执行层级为 Runtime → ControlSession → DeployTask → phase controllers。 + +ControlSession 读取可选 binding(自主运行时直接使用 `None`),构造 `ControlContext`,再调用 +`task.step(state, context)`。Task 按自身需求编排准备、模型执行与正常停止;其 `PolicyController` +使用 `PolicyProcessor` 适配器进行 observation 处理、推理与动作转换,不持有机器人/backend handle, +也不反向引用完整 task。ControlSession 写入 `ControllerStep.command`,负责 open/enable、状态校验、 +health、清理和统计;Runtime 负责会话调度与宿主生命周期,只有 SimulationRuntime 负责 physics/viewer +与仿真世界资源。硬件沿同一路径处理实测状态;ControlSession 与 Runtime 均不解释 task 阶段。 +故障绕过正常 task 停止阶段,立即执行 backend fallback。 仿真和真机可以选择各自合适的具体 device。例如,仿真使用 GLFW keyboard,真机使用遥控器或网络输入;两端 复用同一个 binding 和任务指令语义。 @@ -51,29 +58,10 @@ InputDevice、Binding 与 backend-neutral task command 控制链路 ## Binding 如何进入 Task -运行时通过 `CommandBinding` 获取强类型任务指令: - -```python -from motrix_deploy.runtime.context import PolicyContext - -command = command_binding.read_command(batch_size=1) -context = PolicyContext( - step=step, - elapsed_time_s=elapsed_time_s, - command=command, -) -task.validate_command(context.command) -observation = task.build_observation(state, context) -``` - -部署的机器人 I/O 边界 `RobotInterface` 与可选的 `KeyboardDeviceProvider` / `GamePadDeviceProvider` -capability protocol 定义在 `motrix_deploy.robot.interface`,使用方从该模块导入。provider 提供由自身管理生命周期的 -输入设备,不负责 runtime 插件发现。runtime discovery 仍由 `motrix_deploy.runtime.factory` 通过 -`motrix_deploy.backends` 完成,recipe 仍用 `runtime.backend` 选择插件。 -准备好的 `runtime.robot` 在打开前即可使用。应用显式构造 -`ControlSession(robot=runtime.robot, command_binding=binding, ...)`, -通过 `runtime.bind_control_session(control)` 绑定,再在 `with runtime` 内运行。 -Factory 不通过回调构造 binding 或控制器。 +训练环境中,环境在每个 step 前通过 binding 读取一个指令 batch,交给任务构造观察。部署时,`ControlSession` +持有同一个可选的 `CommandBinding`(自主任务直接传 `None`),把读到的指令放入 `ControlContext.command` +传给任务——两侧消费的是同一种强类型指令。session 与 runtime 的完整组装见 +{doc}`部署教程 `。 例如 MuJoCo deployment 从 runtime 的 `KeyboardDeviceProvider` 取得 `MujocoKeyboardDevice`,再创建 `KeyboardPlanarVelocityBinding`。训练环境则可以为同一个 `PlanarVelocityCommand` 使用 random binding。两条路径最终 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 e9723ec7..523f1b2a 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 @@ -30,7 +30,16 @@ motrix-deploy inspect artifact=artifacts/go2-walk-flat.deploy 导出会选择最近一次 run,并创建 `artifacts/go2-walk-flat.deploy/`。用 `run=` 替代 `env` 可选择指定 run, `output=` 可更改输出位置。已有 artifact 目录不会被覆盖。确认检查结果为 `valid: true`。 -artifact 包含 ONNX 策略、机器人与控制设置,以及任务规格。部署还需要安装 `motrix_deploy_tasks` 插件,提供行走任务实现、 +artifact 目录自包含: + +```text +go2-walk-flat.deploy/ +├── manifest.json # 机器人/控制/任务设置与 payload 校验和 +├── policy/model.onnx # 导出的 actor(含输入归一化) +└── task/ # 任务自有 payload(例如 G1 的 motion NPZ) +``` + +部署还需要安装 `motrix_deploy_tasks` 插件,提供行走任务实现、 部署场景和 Hydra 配方,以及目标对应的运行时插件。配方安装在插件中,在仓库外运行也不需要 workspace 配置路径。 任务 defaults 选择 Hydra 运行时组(`sim` 或 `hardware`),无需另行覆盖运行时组。 顶层公共设置为 `artifact`、`duration_s` 和 `command`。所有目标专用设置直接位于 `runtime` 下; @@ -63,9 +72,9 @@ motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy 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 携带这些设置。 +示例在外部加载模型并注入 task;task 计算机器人命令,control session 负责机器人 I/O 与执行统计, +runtime 负责物理推进、调度与 viewer。[进阶的显式组装](#显式组装-runtime-与任务)一节逐步展示同样的组合方式。 +直接使用 ONNX 文件时需要显式指定任务预处理和动作设置;artifact 则为 CLI 携带这些设置。 ### 选择原生 MotrixSim @@ -116,12 +125,41 @@ motrix-deploy-unitree joint-control enp5s0 FL_thigh_joint 0.9 \ 该工具使用 artifact 中的机器人、增益、时序和限位设置。等待 Start 和 A 后,移动关节、保持、返回默认姿态, 最后以阻尼模式关闭。 -## 进阶说明 +## 部署 G1 WBT 舞蹈 + +G1 全身跟踪舞蹈任务使用同样的导出 → artifact → `motrix-deploy` 流程。artifact 内嵌伺服增益、action scale、 +preparation 增益与时序、安全阈值以及完整的 motion 片段(以 `payloads/motion.npz` payload 形式,校验和记录在 +manifest 中),部署既不需要 Torch 也不需要 RL 环境。导出需要训练 provider 与任务环境包,但**不**执行训练: + +```bash +source .venv/bin/activate +RUN=runs/g1-wbt-dance/motrix/torch/fastsac/ +python scripts/export_deploy.py run="$RUN" \ + output=artifacts/g1-wbt-dance-software-pd.deploy validation.atol=3e-5 + +motrix-deploy task=g1-wbt-dance/sim artifact=artifacts/g1-wbt-dance.deploy +``` + +无窗口验证与 artifact 检查: + +```bash +motrix-deploy task=g1-wbt-dance/sim artifact=artifacts/g1-wbt-dance.deploy \ + runtime.viewer=false +motrix-deploy inspect artifact=artifacts/g1-wbt-dance.deploy +``` + +默认时长来自内嵌的 motion 片段;更短的运行用 `duration_s` 缩短,超长时长会在打开 backend 前被拒绝, +不会循环或保持。无需速度指令:motion 由 artifact 持有。同一 artifact 可运行在 MuJoCo 与 MotrixSim +(`runtime.backend=motrixsim`,配合训练预算使用 `runtime.physics.solver_iterations=3`)。 +它不是真机部署 recipe:当前 Unitree 传输仅支持 Go2,且 sim2sim 成功不代表真机安全。 -Artifact 检查通过 `motrix_deploy.tasks` 解析版本化 task 类并校验其 Pydantic `spec_type`, -不构造 task,也不 import 训练环境或仿真后端。JSON wire format 保持 `{name, config}`。 -Python 使用 `spec.kp`、`spec.action_scale` 等直接属性;action scale 与上下界是规范关节顺序的向量。 -Policy action 上下界先于缩放和默认姿态偏移生效;机器人 position limits 约束最终关节目标。 +任务按真实部署实践编排四个阶段:`prepare`(实测关节 ramp 到默认站立位,就绪门连续满足)、`policy_hold`、 +`playback` 与 `damping`。默认就绪后自动接管;程序化应用可用 `automatic_start=False`、请求方法或附加键盘 +(`p` 启动 policy、`m` 提前启动 motion、`o` 停止)手动分步。注意舞蹈以后倾姿态结束,该姿态只有跟踪中的 +policy 能维持平衡:播放预算完成后 task 直接进入实测 damping 收尾,若要以站立收场,应让 motion 本身以 +站立姿态结束。 + +## 进阶说明 ### 从 YAML 加载 task spec @@ -139,33 +177,15 @@ spec = Go2WalkTaskSpec.model_validate(OmegaConf.to_container(cfg, resolve=True)) `Go2WalkTaskSpec` 使用字段约束和 model validator 校验关节向量长度及上下界顺序,无需单独调用 `validate()`。 完整 deployment manifest 应通过 artifact reader 读取。 -### 启动与数组校验 - -`RobotInterface.enable()` 不接收初始命令。Unitree 自行过渡到 `RobotSpec.default_joint_position`, -目标速度和 feedforward torque 为零,使用解析后的启动增益。`runtime.kp` / `runtime.kd` 覆盖 artifact 增益; -`null` 保留 artifact 增益。启动不通过 task `process_action()` 转换零动作。 -初始状态 safety 与 task termination 检查仍先于 enable,之后重新检查新状态;Start/A 门控不变。 - -`DeployTask` 不声明 `observation_size` 或 `action_size`。policy 输入边界校验实际 observation, -task action 转换边界校验实际 policy 输出。CLI 组装不提前比较 task 维度。manifest tensor specs -保持模型元数据;`DeploymentProfile` 维度仅用于导出时与编译环境交叉校验。 - ### 真机传感器与电机接线 `motrix_deploy_unitree.hardware.UnitreeGo2HardwareCfg` 独立持有不依赖资产的硬件姿态与接线。 `HardwareSensorBinding` 映射 SDK IMU 字段,包括将 `wxyz` 四元数转换为规范 `xyzw`; -`UnitreeMotorBinding` 将规范关节名映射到 SDK 电机索引。这些定义属于硬件插件的 `sensor` 和 -`actuation` 模块,而不是仿真机器人模型。artifact 仍拥有策略关节顺序、默认姿态和限幅。 -导入硬件配置不会提前加载模型资产、仿真器或 SDK,也不伪造 base position 或 linear velocity。 +`UnitreeMotorBinding` 将规范关节名映射到 SDK 电机索引。artifact 仍拥有策略关节顺序、默认姿态和限幅。 ### 选择部署世界 通过 `runtime.deploy_env_id` 选择世界,通过现有 registry ID `runtime.robot_id` 选择机器人。 -CLI 调用 `motrix_deploy.env.assemble_deploy_scene(deploy_env_id, robot_id)`:创建无机器人世界, -通过 `motrix_env_core.registry.make_robot_config(robot_id)` 填入 `scene.objs.robot`。 -core registry 惰性发现已安装的 `motrix_env_core.robots` entry points,训练与部署复用同一机制。部署世界 `flat` 和 `rough` 位于独立的 -`motrix_deploy_tasks.envs` 模块,使用 core 地面/高度场配置,不 import 训练任务、机器人模型或训练 contact 配置。 - 保留同一 Go2 recipe 和 artifact,仅切换世界: ```bash @@ -175,6 +195,9 @@ motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy \ 仅覆写世界不会改变机器人 attachment placement。新地形需要不同净空时应调整 `runtime.robot_translation`; 完整的 `task=go2-walk-rough/sim` recipe 自带对应的 attachment 默认值。 +`RobotCfg.translation`/`rotation` 是与模型固有 base pose 组合一次的 attachment transform: +Go2 固有 base 高度为 0.445 m,identity rotation 下,平地 recipe 的 -0.114 m 偏移得到 0.331 m, +粗糙地形 recipe 的 -0.025 m 偏移得到 0.42 m。 已安装的平地 recipe 使用 ID 选择世界和机器人: @@ -188,32 +211,15 @@ runtime: robot_rotation: [0.0, 0.0, 0.0, 1.0] ``` -`runtime.physics` 与 `runtime.sensor_bindings` 仍是独立设置。MuJoCo 只接收完整 `SceneCfg`, -不选择环境/机器人组合,不组装场景,也不查询训练 registry。程序化 `SimulationRuntimeConfig.scene` 接收同一个完整场景, -机器人只在 `scene.objs.robot` 中声明一次。 - -Placement 只有一个来源:`RobotCfg.translation`(Vec3)与 `rotation`(xyzw)是 attachment transform, -与模型固有 base pose 组合一次,并非绝对世界 root 位姿。Go2 固有 base 高度为 0.445 m;identity rotation 下, -平地 recipe 的 -0.114 m 偏移得到 0.331 m,粗糙地形 recipe 的 -0.025 m 偏移得到 0.42 m。 -runtime 在复制的场景中保留这些变换。`mj_resetData` 恢复编译后的 `model.qpos0`,包括组合后的 root placement; -初始化只用绑定 `RobotSpec` 的关键姿态覆写 articulation 关节。 - -外部世界插件在 `motrix_deploy.envs` entry-point group 中声明零参数 `SceneCfg` factory。 -`motrix_deploy.env.available_deploy_envs()` 无需加载 factory 即可列出已安装 ID; -`create_deploy_env(id)` 只加载选中的 factory,并要求 `scene.objs.robot` 为空。 -core 组装 helper 添加由现有 robot registry 选择的机器人;不引入通用 scene-factory registry,也不通过 -Hydra 实例化场景。CLI 消费 `deploy_env_id`、`robot_id` 与可选 `robot_translation` / `robot_rotation`, -把 attachment overrides 写入 `scene.objs.robot.translation` / `.rotation` 后,再把完整场景传给仿真插件。 -这些应用层选项不是 runtime backend 字段。 - -`SimulationRuntimeConfig` 负责场景、渲染、实时节奏与状态来源设置;其 `physics` 字段使用共享的 -`motrix_env_core.config.sim.SimCfg` 配置 `dt`、`solver_iterations`、`solver_tolerance` 与 `gravity`, -默认值为 `SimCfg(dt=0.002, solver_iterations=100)`。两个 backend compiler 直接消费该字段: -可选 solver/gravity 字段为 `None` 时保留模型来源中的值,显式配置则覆盖。 -YAML 仍使用 `runtime.physics`,与场景、state sensors 保持独立。 -物理参数应在 runtime 构造前设置:runtime 读取编译使用的共享 `SimCfg`;不支持编译后修改 `dt` 来动态调参。 - -程序化应用使用同一 helper,并把结果传给 `SimulationRuntimeConfig`: +`runtime.physics` 使用共享的 `SimCfg`(`dt`、`solver_iterations`、`solver_tolerance`、`gravity`, +默认 `dt=0.002, solver_iterations=100`);可选字段设为 `None` 时保留模型来源中的值。 +物理参数应在 runtime 构造前设置——不支持编译后修改 `dt` 动态调参。 + +`runtime.sensor_bindings` 把状态角色映射到模型内 sensor 名称,只支持 `base_angular_velocity`、 +`base_linear_acceleration` 与 `base_linear_velocity`(`str | None`)。MuJoCo 要求绑定角速度和加速度; +可选的 `base_linear_velocity` 未绑定时保持 `None`。 + +程序化应用组装同一场景并传给 `SimulationRuntimeConfig`: ```python from motrix_deploy.env import assemble_deploy_scene @@ -234,48 +240,42 @@ config = SimulationRuntimeConfig( ) ``` -`SimulationRuntimeConfig.sensor_bindings` 使用 frozen dataclass `SensorBindings`, -而非 Python 字典。它只支持 `base_angular_velocity`、`base_linear_acceleration` 与 -`base_linear_velocity`,类型均为 `str | None`,默认为 `None`。名称必须非空且至少包含 -一个非空白字符;未知角色与非字符串值均被拒绝。YAML `runtime.sensor_bindings` 保留同样的 -映射键,在配置边界通过 `SensorBindings(**mapping)` 转换。字段缺省或显式 `None` 表示 -没有绑定;MuJoCo 在打开机器人时会拒绝这三个必需角色的任一种未绑定状态。 -Backend 仍校验 sensor 是否存在、类型、维度与坐标系;不会为其他角色合成状态, -也不是每个 `RobotState` 字段都通过 sensor 绑定。 +更多世界来自已安装的插件:在 `motrix_deploy.envs` entry-point group 中声明零参数 `SceneCfg` factory, +`motrix_deploy.env.available_deploy_envs()` 可列出已安装 ID。 -### 显式组装 runtime 与控制器 +### 显式组装 runtime 与任务 -Runtime factory 选择 backend 并准备 `runtime.robot`;绑定的 `RobotSpec` 在 `open()` 之前即可读取。 -应用自行创建 task、policy 与 command binding,再显式构造并绑定 control session。 -沿用上面的 `config` 以及应用持有的 `task`、`policy`、`command_binding`: +Runtime factory 选择 backend 并准备 `runtime.robot`;`RobotSpec` 在 `open()` 前即可读取。 +在外部加载模型,然后构造并绑定 session。沿用上面的 `config` 以及应用持有的 +`task_spec`、`model` 与 Go2 `command_binding`: ```python from motrix_deploy.runtime.control import ControlSession from motrix_deploy.runtime.factory import create_simulation_runtime +from motrix_deploy.task import create_task runtime = create_simulation_runtime("mujoco", config) robot = runtime.robot -control = ControlSession( +task = create_task(task_spec, robot.spec, model, steps=50) +session = ControlSession( robot=robot, - task=task, - policy=policy, + controller=task, command_binding=command_binding, period_s=0.02, + state_timeout_s=0.1, ) -runtime.bind_control_session(control) +runtime.bind_session(session) with runtime: - result = runtime.run(steps=50) + result = runtime.run() ``` -依赖机器人契约的 task 应在 runtime 创建后从 `robot.spec` 构造。仿真 runtime 只从 scene 派生 -`RobotSpec`,不接收外部 spec。Artifact 部署由 CLI 在组装 control 前,将 artifact 的机器人契约 -与 `runtime.robot.spec` 对照校验。 -`ControlSession.period_s` 是控制周期的 -唯一配置位置,`SimulationRuntimeConfig` 不重复声明;仿真 runtime 在绑定或解析 session 时校验该周期与 physics timestep -的关系。Runtime 创建不接收控制器构造回调。 -Runtime context 持有世界资源并推进仿真;`ControlSession` 执行共享控制逻辑;`RobotInterface` -负责机器人 I/O,不推进或关闭世界。硬件应用先调用 `create_hardware_runtime(name, config, context)`, -再使用同样的显式构造与绑定流程。直接调用方也可向 `runtime.run(control, steps=...)` 传入 session。 +`create_task(spec, robot, policy, steps=None)` 根据 spec 的任务名解析具体 task class;引用 artifact payload 的 +任务(例如 G1 的 motion NPZ)也通过同一调用获得 artifact。控制周期必须是 physics timestep 的整数倍。 +`command_binding` 默认值为 `None`:G1 舞蹈这类自主任务可省略,Go2 行走则需要外部组装速度 binding, +其值由 task 按速度上下界校验。硬件应用使用 `create_hardware_runtime(name, config, context)` 并按同样方式组装 session。 + +`runtime.run()` 返回 rollout 结果,包含成功标志、退出原因、模型完成步数、墙钟耗时与延迟统计; +仿真运行额外报告 `real_time_factor`。 ### 选择原生 MotrixSim 部署 @@ -288,10 +288,9 @@ motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy \ ``` 同样的 override 适用于 `task=go2-walk-rough/sim`,无需复制 task recipe。程序化应用调用 -`create_simulation_runtime("motrixsim", config)`,然后沿用上面的显式 session 绑定流程。 +`create_simulation_runtime("motrixsim", config)`,然后沿用上面的显式 session 组装流程。 Go2 state mappings 仍为 `gyro`、`accelerometer` 与 `global_linvel`:原生 sensor 校验 local angular -velocity/acceleration 与 world-frame linear velocity。Runtime 持有原生 MSD 编译、model/data、reset、physics -与可选 SDK `RenderApp` 渲染,不创建 MuJoCo runtime 或训练环境。在可用的图形环境中移除 `runtime.viewer=false` +velocity/acceleration 与 world-frame linear velocity。在可用的图形环境中移除 `runtime.viewer=false` 即可启用原生 viewer 与其 keyboard provider。 程序化 ONNX 示例可运行 diff --git a/examples/deploy_to_sim.py b/examples/deploy_to_sim.py index 4ad990ca..0f92627c 100644 --- a/examples/deploy_to_sim.py +++ b/examples/deploy_to_sim.py @@ -1,7 +1,7 @@ # 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.""" +"""Programmatically assemble a Go2 scene, walking task, ONNX model and robot controller.""" import argparse import json @@ -10,12 +10,12 @@ from motrix_robots.unitree import UnitreeGo2Robot from motrix_deploy.contracts import TensorSpec -from motrix_deploy.policy import OnnxPolicyRuntime +from motrix_deploy.policy import OnnxPolicy 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_deploy_tasks.tasks.go2_walk import Go2WalkDeployTask, 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 @@ -39,14 +39,14 @@ def main() -> int: "--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") + parser.add_argument("--steps", type=int, help="Headless session 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( + model = OnnxPolicy( POLICY_PATH, input_spec=TensorSpec(name="obs", shape=(1, 49)), output_spec=TensorSpec(name="actions", shape=(1, 12)), @@ -82,10 +82,10 @@ def main() -> int: runtime = create_simulation_runtime(args.backend, config) robot = runtime.robot - # Explicit preprocessing/action semantics for this walking policy. These must + # Explicit preprocessing/action semantics for this walking controller. These must # match its training configuration; a raw ONNX file does not describe them. action_scale = 0.25 - task = Go2WalkDeployTaskV1( + task = Go2WalkDeployTask( Go2WalkTaskSpec( action_scale=[action_scale] * robot.spec.joint_count, kp=[35.0] * robot.spec.joint_count, @@ -99,9 +99,10 @@ def main() -> int: gait_frequency_hz=2.0, standing_threshold=0.05, termination_min_up_z=0.5, - termination_min_base_height=None, ), robot.spec, + model, + steps=args.steps, ) if not args.headless and isinstance(runtime, KeyboardDeviceProvider): command_binding = KeyboardPlanarVelocityBinding( @@ -112,17 +113,13 @@ def main() -> int: 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, + period_s = 0.02 + session = ControlSession( + controller=task, robot=robot, command_binding=command_binding, period_s=period_s, state_timeout_s=0.1 ) - runtime.bind_control_session(control) + runtime.bind_session(session) with runtime: - result = runtime.run(steps=args.steps) + result = runtime.run() print(json.dumps(result.to_dict(), indent=2, sort_keys=True)) return 0 if result.success else 1 diff --git a/motrix_deploy/src/motrix_deploy/artifact/io.py b/motrix_deploy/src/motrix_deploy/artifact/io.py index d06500bf..83d00e03 100644 --- a/motrix_deploy/src/motrix_deploy/artifact/io.py +++ b/motrix_deploy/src/motrix_deploy/artifact/io.py @@ -72,20 +72,25 @@ def write_artifact( parent = target.parent parent.mkdir(parents=True, exist_ok=True) expected_path = _safe_relative_path(manifest.policy.payload_path, field_path="policy.payload_path") - if set(payloads) != {str(expected_path)}: - raise ArtifactError(f"payload keys must be exactly [{str(expected_path)!r}], got {sorted(payloads)!r}") - payload = payloads[str(expected_path)] - if not isinstance(payload, bytes): - raise ArtifactError(f"payload {expected_path} must be bytes") - digest = sha256_bytes(payload) - if digest != manifest.policy.sha256: - raise ArtifactError(f"policy.sha256 mismatch while writing: expected {manifest.policy.sha256}, got {digest}") + expected_payloads = {str(expected_path): manifest.policy.sha256} + for payload in manifest.payloads: + relative = _safe_relative_path(payload.path, field_path="payloads.path") + expected_payloads[str(relative)] = payload.sha256 + if set(payloads) != set(expected_payloads): + raise ArtifactError(f"payload keys must be exactly {sorted(expected_payloads)}, got {sorted(payloads)!r}") + for relative, expected_sha in expected_payloads.items(): + if not isinstance(payloads[relative], bytes): + raise ArtifactError(f"payload {relative} must be bytes") + digest = sha256_bytes(payloads[relative]) + if digest != expected_sha: + raise ArtifactError(f"sha256 mismatch for payload {relative}: expected {expected_sha}, got {digest}") staging = Path(tempfile.mkdtemp(prefix=f".{target.name}.staging-", dir=parent)) try: - payload_path = _payload_path(staging, expected_path) - payload_path.parent.mkdir(parents=True, exist_ok=True) - payload_path.write_bytes(payload) + for relative in sorted(expected_payloads): + payload_path = _payload_path(staging, PurePosixPath(relative)) + payload_path.parent.mkdir(parents=True, exist_ok=True) + payload_path.write_bytes(payloads[relative]) manifest_path = staging / MANIFEST_NAME manifest_path.write_text( json.dumps(manifest.to_dict(), indent=2, sort_keys=True, allow_nan=False) + "\n", @@ -111,6 +116,14 @@ def read_artifact(root: str | Path) -> Artifact: except (OSError, UnicodeError, json.JSONDecodeError) as error: raise ArtifactError(f"cannot parse artifact manifest {manifest_path}: {error}") from error manifest = DeploymentManifest.from_dict(raw_manifest) + for payload in manifest.payloads: + relative = _safe_relative_path(payload.path, field_path="payloads.path") + payload_file = _payload_path(artifact_root, relative) + if not payload_file.is_file(): + raise ArtifactError(f"artifact payload is missing: {relative}") + digest = hashlib.sha256(payload_file.read_bytes()).hexdigest() + if digest != payload.sha256: + raise ArtifactError(f"payload sha256 mismatch for {relative}: expected {payload.sha256}, got {digest}") relative = _safe_relative_path(manifest.policy.payload_path, field_path="policy.payload_path") policy_path = _payload_path(artifact_root, relative) if not policy_path.is_file(): diff --git a/motrix_deploy/src/motrix_deploy/artifact/schema.py b/motrix_deploy/src/motrix_deploy/artifact/schema.py index 3f64c6ef..6c5c52f1 100644 --- a/motrix_deploy/src/motrix_deploy/artifact/schema.py +++ b/motrix_deploy/src/motrix_deploy/artifact/schema.py @@ -27,13 +27,14 @@ def _mapping(value: object, path: str) -> Mapping[str, Any]: return value -def _keys(value: Mapping[str, Any], *, path: str, required: set[str]) -> None: +def _keys(value: Mapping[str, Any], *, path: str, required: set[str], optional: set[str] | None = None) -> None: missing = sorted(required - value.keys()) if missing: raise ValidationError(path, f"required fields {sorted(required)}", f"missing {missing}") - unknown = sorted(value.keys() - required) + allowed = required | (optional or set()) + unknown = sorted(value.keys() - allowed) if unknown: - raise ValidationError(path, f"only fields {sorted(required)}", f"unknown {unknown}") + raise ValidationError(path, f"only fields {sorted(allowed)}", f"unknown {unknown}") def _string(value: object, path: str) -> str: @@ -279,6 +280,14 @@ def from_dict(cls, value: object) -> "ControlSpec": return cls(**data) +@dataclass(frozen=True) +class PayloadSpec: + """A task-owned binary payload (e.g. a motion-clip NPZ) with its checksum.""" + + path: str + sha256: str + + @dataclass(frozen=True) class DeploymentManifest: """Top-level deployment manifest with strict v1 component versions.""" @@ -289,6 +298,7 @@ class DeploymentManifest: robot: RobotSpec task: TaskSpec control: ControlSpec + payloads: tuple[PayloadSpec, ...] = () def __post_init__(self) -> None: if self.schema_version != SCHEMA_VERSION: @@ -306,13 +316,19 @@ def to_dict(self) -> dict[str, Any]: "robot": _robot_to_dict(self.robot), "task": self.task.to_dict(), "control": self.control.to_dict(), + "payloads": [payload.__dict__ for payload in self.payloads], } @classmethod def from_dict(cls, value: object) -> "DeploymentManifest": data = _mapping(value, "manifest") required = {"schema_version", "source", "policy", "robot", "task", "control"} - _keys(data, path="manifest", required=required) + _keys(data, path="manifest", required=required, optional={"payloads"}) + payloads = [] + for entry in data.get("payloads", ()): + payload = _mapping(entry, "manifest.payloads") + _keys(payload, path="manifest.payloads", required={"path", "sha256"}) + payloads.append(PayloadSpec(path=payload["path"], sha256=payload["sha256"])) return cls( schema_version=data["schema_version"], source=SourceSpec.from_dict(data["source"]), @@ -320,4 +336,5 @@ def from_dict(cls, value: object) -> "DeploymentManifest": robot=_robot_from_dict(data["robot"]), task=TaskSpec.from_dict(data["task"]), control=ControlSpec.from_dict(data["control"]), + payloads=tuple(payloads), ) diff --git a/motrix_deploy/src/motrix_deploy/cli.py b/motrix_deploy/src/motrix_deploy/cli.py index f1e1bd39..dd054cf3 100644 --- a/motrix_deploy/src/motrix_deploy/cli.py +++ b/motrix_deploy/src/motrix_deploy/cli.py @@ -20,14 +20,14 @@ 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.control import ControlSession from motrix_deploy.runtime.factory import RuntimeCreateContext, create_hardware_runtime, create_simulation_runtime -from motrix_deploy.task import create_task +from motrix_deploy.task import create_task, load_task_type def run(cfg: DeployRunConfig) -> int: """Run one deployment rollout from typed application configuration.""" - from motrix_deploy.policy import OnnxPolicyRuntime - from motrix_deploy.runtime.control import ControlSession + from motrix_deploy.policy import OnnxPolicy from motrix_env_core.input import ( BoundedGamePadPlanarVelocityBinding, ConstantPlanarVelocityBinding, @@ -36,8 +36,17 @@ 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) + task_type = load_task_type(manifest.task.name) + default_duration_s = task_type.default_duration_s(manifest.task) + duration_s = cfg.duration_s if cfg.duration_s is not None else default_duration_s + steps = _duration_steps(duration_s, manifest.control.period_s) + if default_duration_s is not None and duration_s > default_duration_s + 1e-9: + raise ValueError("duration_s exceeds the task's finite playback budget") + task_period = getattr(manifest.task, "period_s", manifest.control.period_s) + if not math.isclose(task_period, manifest.control.period_s, rel_tol=0, abs_tol=1e-9): + raise ValueError("task period_s must equal control.period_s") + policy = OnnxPolicy(artifact.policy_path, manifest.policy.input, manifest.policy.output) + task = create_task(manifest.task, manifest.robot, policy, steps=steps, artifact=artifact) simulation = cfg.runtime["kind"] == "simulation" viewer = cfg.runtime["viewer"] if simulation else False realtime = cfg.runtime["realtime"] if simulation else True @@ -76,13 +85,18 @@ def run(cfg: DeployRunConfig) -> int: except ValidationError: runtime.close() raise + command_binding = None 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(runtime, KeyboardDeviceProvider): + if command_lower is None and command_upper is None: + if command_config: + raise ValueError("Autonomous task playback does not accept planar velocity commands") + task.validate_command(None) + elif command_lower is None or command_upper is None: + raise ValueError(f"Task {manifest.task.name!r} must provide both velocity bounds") + elif viewer and isinstance(runtime, KeyboardDeviceProvider): command_binding = KeyboardPlanarVelocityBinding( runtime.get_keyboard_device(), command_lower=command_lower, @@ -113,17 +127,16 @@ 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") - control = ControlSession( + session = ControlSession( + controller=task, robot=backend, - task=task, - policy=OnnxPolicyRuntime(artifact.policy_path, manifest.policy.input, manifest.policy.output), command_binding=command_binding, period_s=manifest.control.period_s, state_timeout_s=manifest.control.state_timeout_s, ) - runtime.bind_control_session(control) + runtime.bind_session(session) with runtime: - result = runtime.run(steps=steps) + result = runtime.run() print(json.dumps(result.to_dict(), indent=2, sort_keys=True)) return 0 if result.success else 1 diff --git a/motrix_deploy/src/motrix_deploy/contracts.py b/motrix_deploy/src/motrix_deploy/contracts.py index 708a836f..8aabd6a4 100644 --- a/motrix_deploy/src/motrix_deploy/contracts.py +++ b/motrix_deploy/src/motrix_deploy/contracts.py @@ -3,8 +3,7 @@ """Framework-independent deployment data contracts.""" -from collections.abc import Mapping -from dataclasses import dataclass, field +from dataclasses import dataclass from enum import Enum from typing import TypeAlias @@ -130,9 +129,8 @@ class RobotState: base_orientation_xyzw: FloatArray base_angular_velocity: FloatArray base_linear_acceleration: FloatArray - base_position: FloatArray | None = None + # Optional world-frame sensor/estimator output; unavailable backends leave it unset. base_linear_velocity: FloatArray | None = None - extras: Mapping[str, FloatArray] = field(default_factory=dict) def __post_init__(self) -> None: if self.sample_time_ns < 0: @@ -166,8 +164,6 @@ def __post_init__(self) -> None: path="state.base_linear_acceleration", shape=(3,), ) - if self.base_position is not None: - _validate_float32_array(self.base_position, path="state.base_position", shape=(3,)) if self.base_linear_velocity is not None: _validate_float32_array( self.base_linear_velocity, @@ -177,12 +173,6 @@ def __post_init__(self) -> None: norm = np.linalg.norm(self.base_orientation_xyzw) if not np.isclose(norm, 1.0, atol=1e-4): raise ValidationError("state.base_orientation_xyzw", "unit quaternion", norm) - for name, value in self.extras.items(): - if not isinstance(name, str) or not name: - raise ValidationError("state.extras", "non-empty string keys", name) - if not isinstance(value, np.ndarray): - raise ValidationError(f"state.extras.{name}", "a numpy array", type(value).__name__) - _validate_float32_array(value, path=f"state.extras.{name}", shape=value.shape) class JointControlMode(str, Enum): @@ -256,8 +246,8 @@ class RobotCapabilities: Attributes: control_modes: Canonical joint command modes accepted by the backend. - state_fields: ``RobotState`` fields populated by every state sample. - extra_sensors: Additional entries available in ``RobotState.extras``, mapped to their fixed shapes. + state_fields: ``RobotState`` fields supported by the backend, independently of sensor bindings. + Optional fields can be ``None`` in samples when their sensor is not bound. 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. requires_enable: Whether ``RobotInterface.enable()`` must complete before policy commands are accepted. @@ -266,7 +256,6 @@ class RobotCapabilities: 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 @@ -289,11 +278,6 @@ def __post_init__(self) -> None: raise ValidationError("capabilities.max_command_rate_hz", "a positive value", self.max_command_rate_hz) if not self.stop_semantics: raise ValidationError("capabilities.stop_semantics", "a non-empty string", self.stop_semantics) - for name, shape in self.extra_sensors.items(): - if not isinstance(name, str) or not name: - raise ValidationError("capabilities.extra_sensors", "non-empty string keys", name) - if not isinstance(shape, tuple) or any(not isinstance(size, int) or size <= 0 for size in shape): - raise ValidationError(f"capabilities.extra_sensors.{name}", "a tuple of positive dimensions", shape) @dataclass(frozen=True) diff --git a/motrix_deploy/src/motrix_deploy/errors.py b/motrix_deploy/src/motrix_deploy/errors.py index 4ca001b7..74e10987 100644 --- a/motrix_deploy/src/motrix_deploy/errors.py +++ b/motrix_deploy/src/motrix_deploy/errors.py @@ -24,3 +24,9 @@ class EmergencyStopError(RuntimeError): class LieDownRequestedError(RuntimeError): """A physical input requested the backend's configured lie-down shutdown.""" + + +class ControlFailure(RuntimeError): + def __init__(self, reason: str, message: str) -> None: + self.reason = reason + super().__init__(message) diff --git a/motrix_deploy/src/motrix_deploy/policy/__init__.py b/motrix_deploy/src/motrix_deploy/policy/__init__.py index 7d91d05a..58f6ba1c 100644 --- a/motrix_deploy/src/motrix_deploy/policy/__init__.py +++ b/motrix_deploy/src/motrix_deploy/policy/__init__.py @@ -13,7 +13,7 @@ from motrix_deploy.errors import ValidationError -class PolicyRuntime(ABC): +class Policy(ABC): """Decision runtime for a task's vector observation/action interface. Implementations may use networks, planners, or fixed decisions. Tensor names @@ -28,7 +28,7 @@ def infer(self, observation: FloatArray) -> FloatArray: """Infer one unbatched action.""" -class NoOpPolicyRuntime(PolicyRuntime): +class NoOpPolicy(Policy): """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, @@ -36,8 +36,6 @@ class NoOpPolicyRuntime(PolicyRuntime): """ 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: @@ -45,7 +43,7 @@ def infer(self, observation: FloatArray) -> FloatArray: return self._action.copy() -class OnnxPolicyRuntime(PolicyRuntime): +class OnnxPolicy(Policy): """Single-input, single-output ONNX Runtime policy.""" def __init__( @@ -111,4 +109,4 @@ def infer(self, observation: FloatArray) -> FloatArray: return float32_array(output[0], path="policy.output", shape=(self.output_spec.shape[1],)) -__all__ = ["NoOpPolicyRuntime", "OnnxPolicyRuntime", "PolicyRuntime"] +__all__ = ["NoOpPolicy", "OnnxPolicy", "Policy"] diff --git a/motrix_deploy/src/motrix_deploy/policy/processing.py b/motrix_deploy/src/motrix_deploy/policy/processing.py new file mode 100644 index 00000000..c32dae45 --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/policy/processing.py @@ -0,0 +1,36 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Task-specific preprocessing surrounding observation-to-action inference.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod +from typing import TYPE_CHECKING, Generic, TypeVar + +from motrix_deploy.contracts import FloatArray, RobotCommand, RobotState + +if TYPE_CHECKING: + from motrix_deploy.runtime.context import ControlContext + +CommandT = TypeVar("CommandT") + + +class PolicyProcessor(ABC, Generic[CommandT]): + """Own observation/action history and policy input validity, not task phases.""" + + @abstractmethod + def reset(self, state: RobotState, context: ControlContext[CommandT]) -> None: ... + + @abstractmethod + def build_observation(self, state: RobotState, context: ControlContext[CommandT]) -> FloatArray: ... + + @abstractmethod + def process_action(self, action: FloatArray) -> RobotCommand: ... + + @abstractmethod + def validate_command(self, command: CommandT | None) -> None: ... + + def check_termination(self, state: RobotState) -> str | None: + """Return a policy termination reason, or None when no check is required.""" + return None diff --git a/motrix_deploy/src/motrix_deploy/profile.py b/motrix_deploy/src/motrix_deploy/profile.py index df68d774..7788015e 100644 --- a/motrix_deploy/src/motrix_deploy/profile.py +++ b/motrix_deploy/src/motrix_deploy/profile.py @@ -3,8 +3,8 @@ """Task-agnostic result of compiling one training environment for deployment.""" -from collections.abc import Callable -from dataclasses import dataclass +from collections.abc import Callable, Mapping +from dataclasses import dataclass, field from motrix_deploy.artifact.schema import ControlSpec, TaskSpec from motrix_deploy.contracts import RobotSpec @@ -20,6 +20,8 @@ class DeploymentProfile: # Training-space dimensions used only to validate ONNX export, not serialized in TaskSpec. observation_size: int action_size: int + # Task-owned binary payloads (path -> bytes) written into the artifact next to the policy. + payloads: Mapping[str, bytes] = field(default_factory=dict) ProfileCompiler = Callable[[str], DeploymentProfile] diff --git a/motrix_deploy/src/motrix_deploy/runtime/__init__.py b/motrix_deploy/src/motrix_deploy/runtime/__init__.py index 560dddbf..fcb8e6e0 100644 --- a/motrix_deploy/src/motrix_deploy/runtime/__init__.py +++ b/motrix_deploy/src/motrix_deploy/runtime/__init__.py @@ -5,8 +5,8 @@ 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.context import ControlContext +from motrix_deploy.runtime.control import ControlSession, PolicyController from motrix_deploy.runtime.factory import ( RuntimeCreateContext, create_hardware_runtime, @@ -14,18 +14,23 @@ registered_runtimes, ) from motrix_deploy.runtime.hardware import HardwareRuntime -from motrix_deploy.runtime.result import RolloutResult +from motrix_deploy.runtime.lifecycle import Controller, ControllerStep +from motrix_deploy.runtime.result import RolloutResult, SimulationRolloutResult from motrix_deploy.runtime.scheduler import FixedStepScheduler, LoopScheduler, RealtimeScheduler __all__ = [ "ControlSession", + "PolicyController", + "ControllerStep", + "Controller", "DeploymentRuntime", "FixedStepScheduler", "HardwareRuntime", "LoopScheduler", - "PolicyContext", + "ControlContext", "RealtimeScheduler", "RolloutResult", + "SimulationRolloutResult", "SensorBindings", "SimulationRuntimeConfig", "SimulationRuntime", diff --git a/motrix_deploy/src/motrix_deploy/runtime/base.py b/motrix_deploy/src/motrix_deploy/runtime/base.py index 3fafaef4..3e326d4d 100644 --- a/motrix_deploy/src/motrix_deploy/runtime/base.py +++ b/motrix_deploy/src/motrix_deploy/runtime/base.py @@ -1,66 +1,48 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Deployment runtime lifecycle contracts.""" +"""Runtime resources, scheduling and physics advancement of a bound control session.""" from abc import ABC, abstractmethod +from dataclasses import fields from typing import TYPE_CHECKING from motrix_deploy.errors import ValidationError +from motrix_deploy.runtime.control import ControlSession +from motrix_deploy.runtime.result import RolloutResult, SimulationRolloutResult +from motrix_deploy.runtime.scheduler import LoopScheduler 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.""" + """Own world resources and scheduling, independent of controller semantics.""" def __init__(self) -> None: self._opened = False - self._control: ControlSession | None = None + self.session: ControlSession | None = None + self._used = False - @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 + def bind_session(self, session: ControlSession) -> None: + if self.session is not None and self.session is not session: + raise ValidationError("runtime.session", "the already bound control session", session) + if session.robot is not self.robot: + raise ValidationError("runtime.session.robot", self.robot, session.robot) + self.session = session @property @abstractmethod - def robot(self) -> "RobotInterface": - """Robot interface associated with this runtime.""" - raise NotImplementedError + def robot(self) -> "RobotInterface": ... @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": @@ -68,16 +50,91 @@ def __enter__(self) -> "DeploymentRuntime": return self def __exit__(self, exc_type, exc_value, traceback) -> None: - del exc_type, exc_value, traceback self.close() + def _advance_interval(self) -> None: + """Apply host-specific execution after an accepted command.""" + + def _check_running(self) -> None: + """Check whether the host permits another control interval.""" + + def _run_session(self, scheduler: LoopScheduler) -> RolloutResult: + if self.session is None: + raise ValidationError("runtime.session", "a bound control session", None) + if self._used: + raise ValidationError("runtime.session", "a single-use execution", "already executed") + self._used = True + session = self.session + try: + if session.start(): + scheduler.reset() + interval_step = 0 + while session.active: + self._check_running() + if not session.tick(elapsed_time_s=scheduler.elapsed_time_s(interval_step)): + break + self._advance_interval() + interval_step += 1 + scheduler.wait(interval_step) + except (Exception, KeyboardInterrupt) as error: + session.fail(error) + finally: + session.stop() + return session.result(overrun_count=scheduler.overrun_count) + @abstractmethod - def run(self, control: "ControlSession | None" = None, *, steps: int | None = None) -> "RolloutResult": - """Run a control session and return its structured result.""" + def run(self) -> RolloutResult: ... class SimulationRuntime(DeploymentRuntime, ABC): - """Base for runtimes that own simulation advancement and robot creation.""" + """Runtime that additionally owns simulation advancement and viewer resources.""" + + def __init__(self) -> None: + super().__init__() + self._policy_simulation_time_s = 0.0 + + def bind_session(self, session: ControlSession) -> None: + if self.session is not None and self.session is not session: + raise ValidationError("runtime.session", "the already bound control session", session) + if session.robot is not self.robot: + raise ValidationError("runtime.session.robot", self.robot, session.robot) + self._configure_session_period(session.period_s) + super().bind_session(session) + + def _configure_session_period(self, period_s: float) -> None: + """Configure simulation substeps for the control interval.""" + + @abstractmethod + def advance_control_period(self) -> None: + """Advance physics through one control interval.""" + + @abstractmethod + def _physics_time_s(self) -> float: + """Read the physical simulation clock.""" + + def _advance_interval(self) -> None: + assert self.session is not None + before = self._physics_time_s() + try: + self.advance_control_period() + finally: + if self.session.policy_tick: + self._policy_simulation_time_s += self._physics_time_s() - before + + def _run_session(self, scheduler: LoopScheduler) -> SimulationRolloutResult: + self._policy_simulation_time_s = 0.0 + result = super()._run_session(scheduler) + return SimulationRolloutResult( + **{item.name: getattr(result, item.name) for item in fields(RolloutResult)}, + policy_simulation_time_s=self._policy_simulation_time_s, + real_time_factor=self._policy_simulation_time_s / result.wall_time_s if result.wall_time_s > 0 else 0.0, + ) + + def _check_running(self) -> None: + self.check_viewer_running() + + def check_viewer_running(self) -> None: + """Reject viewer shutdown; headless simulation hosts need no check.""" __all__ = ["DeploymentRuntime", "SimulationRuntime"] diff --git a/motrix_deploy/src/motrix_deploy/runtime/context.py b/motrix_deploy/src/motrix_deploy/runtime/context.py index bff09f86..cf384396 100644 --- a/motrix_deploy/src/motrix_deploy/runtime/context.py +++ b/motrix_deploy/src/motrix_deploy/runtime/context.py @@ -1,31 +1,22 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Task-typed per-tick policy context.""" +"""Task-typed per-tick controller inputs.""" from dataclasses import dataclass from typing import Generic, TypeVar -import numpy as np - -from motrix_deploy.errors import ValidationError - CommandT = TypeVar("CommandT") @dataclass(frozen=True) -class PolicyContext(Generic[CommandT]): +class ControlContext(Generic[CommandT]): """Deterministic per-tick inputs that do not come from robot state.""" step: int elapsed_time_s: float - command: CommandT - - def __post_init__(self) -> None: - if not isinstance(self.step, int) or isinstance(self.step, bool) or self.step < 0: - raise ValidationError("context.step", "a non-negative integer", self.step) - if not np.isfinite(self.elapsed_time_s) or self.elapsed_time_s < 0: - raise ValidationError("context.elapsed_time_s", "a non-negative finite value", self.elapsed_time_s) + command: CommandT | None + dt_s: float -__all__ = ["PolicyContext"] +__all__ = ["ControlContext"] diff --git a/motrix_deploy/src/motrix_deploy/runtime/control.py b/motrix_deploy/src/motrix_deploy/runtime/control.py index 0b113e92..e2148e06 100644 --- a/motrix_deploy/src/motrix_deploy/runtime/control.py +++ b/motrix_deploy/src/motrix_deploy/runtime/control.py @@ -1,294 +1,301 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Backend-neutral control executor, independent of scheduling and physics.""" +"""Control sessions and policy-driven robot command generation.""" import hashlib import time -from typing import TYPE_CHECKING, Generic, TypeVar +from typing import 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.contracts import JointServoCommand, RobotState +from motrix_deploy.errors import ControlFailure, EmergencyStopError, LieDownRequestedError, ValidationError +from motrix_deploy.policy import Policy +from motrix_deploy.policy.processing import PolicyProcessor from motrix_deploy.robot.interface import RobotInterface -from motrix_deploy.runtime.context import PolicyContext +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy.runtime.lifecycle import Controller, ControllerStep 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 PolicyController(Generic[CommandT]): + """Map state and command input to robot commands through an observation-to-action policy. + Policy I/O preprocessing and action conversion surround pure policy inference; no + robot handles, I/O or physical timing are owned here. + """ -class ControlSession(Generic[CommandT]): - """Own one control lifecycle; callers own scheduling and simulation advancement. + def __init__(self, policy_io: PolicyProcessor[CommandT], policy: Policy, steps: int | None = None): + self.policy_io = policy_io + self.policy = policy + self.steps = steps + self.completed_steps = 0 + self._initial_state: RobotState | None = None + self._initialized = False + self._stop_requested = False + self._time_origin_s: float | None = None - 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 _check_termination(self, state: RobotState) -> None: + try: + reason = self.policy_io.check_termination(state) + except Exception as error: + raise ControlFailure("task_termination_error", str(error)) from error + if reason is not None: + raise ControlFailure("task_terminated", reason) + + def reset(self, state: RobotState) -> None: + self._check_termination(state) + self.policy.reset() + self._initial_state = state + self._initialized = False + self._stop_requested = False + self.completed_steps = 0 + self._time_origin_s = None + + def request_stop(self) -> None: + self._stop_requested = True + + def step(self, state: RobotState, context: ControlContext[CommandT]) -> ControllerStep: + # Check the measured result of the last interval before finite completion. + self._check_termination(state) + if self._stop_requested or (self.steps is not None and self.completed_steps >= self.steps): + return ControllerStep( + complete=True, + success=not self._stop_requested or self.steps is not None and self.completed_steps >= self.steps, + ) + try: + self.policy_io.validate_command(context.command) + except Exception as error: + raise ControlFailure("input_error", str(error)) from error + if self._time_origin_s is None: + self._time_origin_s = context.elapsed_time_s + local = ControlContext( + self.completed_steps, context.elapsed_time_s - self._time_origin_s, context.command, context.dt_s + ) + if not self._initialized: + assert self._initial_state is not None + self.policy_io.reset(self._initial_state, local) + self._initialized = True + latency_ns = {} + phase_start = time.monotonic_ns() + try: + observation = self.policy_io.build_observation(state, local) + except Exception as error: + raise ControlFailure("invalid_observation", str(error)) from error + latency_ns["observation"] = time.monotonic_ns() - phase_start + phase_start = time.monotonic_ns() + try: + raw_action = self.policy.infer(observation) + except Exception as error: + raise ControlFailure("policy_error", str(error)) from error + latency_ns["inference"] = time.monotonic_ns() - phase_start + phase_start = time.monotonic_ns() + try: + command = self.policy_io.process_action(raw_action) + except Exception as error: + raise ControlFailure("invalid_action", str(error)) from error + latency_ns["action"] = time.monotonic_ns() - phase_start + trace = self.completed_steps.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, + ): + trace += value.tobytes(order="C") + self.completed_steps += 1 + return ControllerStep(command=command, policy_tick=True, trace_bytes=trace, latency_ns=latency_ns) + + +class ControlSession(Generic[CommandT]): + """Execute a bound robot controller without interpreting its internal states.""" def __init__( self, *, robot: RobotInterface, - task: "DeployTask[CommandT]", - policy: PolicyRuntime, - command_binding: CommandBinding[CommandT], + controller: Controller[CommandT], + command_binding: CommandBinding[CommandT] | None = None, 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)) + raise ValidationError("session.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.controller = controller 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._finished = False + self._success = False + self._previous: int | None = None + self._interval_step = 0 + self.completed_steps = 0 + self.policy_tick = False self._trace = hashlib.sha256() + self._latency = LatencyRecorder() 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 + self._reason = "completed" + self._message: str | None = None @property def active(self) -> bool: - return self._started and not self._stopped and self._exit_reason == "completed" + return self._started and not self._stopped and not self._finished and self._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": + if self._started or self._stopped or self._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() + rate = self.robot.capabilities.max_command_rate_hz + if rate is not None and 1 / self.period_s > rate + 1e-9: + raise ValidationError("session.command_rate_hz", f"<= {rate}", 1 / self.period_s) + state = self._read_state(None) + self._previous = state.sample_time_ns + self.controller.reset(state) 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 + raise ControlFailure("backend_enable_error", str(error)) from error + state = self._read_state(self._previous) + self._previous = state.sample_time_ns + self.controller.reset(state) self._started = True except (Exception, KeyboardInterrupt) as error: self.fail(error) return self.active + def _read_state(self, previous: int | None) -> "RobotState": + try: + state = self.robot.read_state(self.state_timeout_s) + except TimeoutError as error: + raise ControlFailure("state_timeout", str(error)) from error + except (EmergencyStopError, LieDownRequestedError): + raise + except Exception as error: + raise ControlFailure("backend_read_error", str(error)) from error + try: + expected = (self.robot.spec.joint_count,) + if state.joint_position.shape != expected or state.joint_velocity.shape != expected: + raise ControlFailure("invalid_state", f"joint shape must be {expected}") + if previous is not None and state.sample_time_ns < previous: + raise ControlFailure("invalid_state", "state sample timestamp moved backwards") + age_s = (time.monotonic_ns() - state.receive_time_ns) / 1e9 + if age_s > self.state_timeout_s: + raise ControlFailure("state_timeout", f"state age {age_s:.6f}s exceeds {self.state_timeout_s:.6f}s") + except ControlFailure: + raise + except Exception as error: + raise ControlFailure("invalid_state", str(error)) from error + self._check_health() + return state + + def _check_health(self) -> None: + health = self.robot.health() + if not health.healthy: + raise ControlFailure("backend_unhealthy", health.reason) + 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 + self.policy_tick = False 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() + phase = time.monotonic_ns() + state = self._read_state(self._previous) + self._previous = state.sample_time_ns + self._latency.add("read", time.monotonic_ns() - phase) + phase = time.monotonic_ns() try: - raw_action = self.policy.infer(observation) + command = self.command_binding.read_command(batch_size=1) if self.command_binding is not None else None 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) + raise ControlFailure("input_error", str(error)) from error + context = ControlContext(self._interval_step, elapsed_time_s, command, self.period_s) + self._latency.add("input", time.monotonic_ns() - phase) + decision = self.controller.step(state, context) + if decision.error is not None: + raise ControlFailure("controller_error", decision.error) + if decision.complete: + self._finished = True + self._success = decision.success + return False + if decision.command is not None: + if decision.command.mode not in self.robot.capabilities.control_modes: + raise ControlFailure("invalid_action", f"unsupported control mode {decision.command.mode}") + phase = time.monotonic_ns() + try: + self.robot.write_command(decision.command) + except (EmergencyStopError, LieDownRequestedError): + raise + except Exception as error: + raise ControlFailure("backend_write_error", str(error)) from error + self._latency.add("write", time.monotonic_ns() - phase) + self._check_health() + self.policy_tick = decision.policy_tick + if self.policy_tick: + self.completed_steps += 1 + self._trace.update(decision.trace_bytes) + for name, value in decision.latency_ns.items(): + self._latency.add(name, value) + self._interval_step += 1 + self._latency.add("loop", time.monotonic_ns() - loop_start) except (Exception, KeyboardInterrupt) as error: self.fail(error) return False return True + def request_stop(self) -> None: + self.controller.request_stop() + 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": + if self._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" + self._reason, self._message = "interrupted", "rollout interrupted by user" + elif isinstance(error, EmergencyStopError): + self._reason, self._message = "emergency_stop", str(error) elif isinstance(error, LieDownRequestedError): - self._exit_reason = "lie_down" - elif isinstance(error, _LoopFailure): - self._exit_reason = error.reason + self._reason, self._message = "lie_down", str(error) + elif isinstance(error, ControlFailure): + self._reason, self._message = error.reason, str(error) elif isinstance(error, ValidationError): - self._exit_reason = "validation_error" + self._reason, self._message = "validation_error", str(error) else: - self._exit_reason = "backend_error" - self._error_message = str(error) + self._reason, self._message = "backend_error", 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) + if self._reason == "completed": + self._reason, self._message = "cleanup_error", str(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.""" + def result(self, *, overrun_count: int = 0) -> RolloutResult: 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 + wall_s = (end_ns - self._start_ns) / 1e9 if self._start_ns is not None else 0.0 return RolloutResult( - success=self._exit_reason == "completed" and steps is not None and self.completed_steps == steps, - exit_reason=self._exit_reason, + success=self._reason == "completed" and self._finished and self._success, + exit_reason=self._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, + wall_time_s=wall_s, overrun_count=overrun_count, trace_sha256=self._trace.hexdigest(), - error=self._error_message, - latency=self._recorder.summarize(), + error=self._message, + latency=self._latency.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/hardware.py b/motrix_deploy/src/motrix_deploy/runtime/hardware.py index ff2d0a82..ddd3b0f3 100644 --- a/motrix_deploy/src/motrix_deploy/runtime/hardware.py +++ b/motrix_deploy/src/motrix_deploy/runtime/hardware.py @@ -6,17 +6,12 @@ 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. - """ + """Own the hardware host and pace bound control session without advancing physics.""" def __init__( self, @@ -34,35 +29,21 @@ def __init__( 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) + def run(self) -> RolloutResult: + if self.session is None: + raise ValidationError("runtime.session", "a bound control session", None) 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) + scheduler = ( + RealtimeScheduler(self.session.period_s) if self.realtime else FixedStepScheduler(self.session.period_s) + ) + elif scheduler.period_s != self.session.period_s: + raise ValidationError("runtime.scheduler.period_s", self.session.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) + return self._run_session(scheduler) 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/lifecycle.py b/motrix_deploy/src/motrix_deploy/runtime/lifecycle.py new file mode 100644 index 00000000..8990d5b3 --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/runtime/lifecycle.py @@ -0,0 +1,36 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Robot-level controller decisions, independent of backend resources and scheduling.""" + +from dataclasses import dataclass, field +from typing import Protocol, TypeVar + +from motrix_deploy.contracts import RobotCommand, RobotState +from motrix_deploy.runtime.context import ControlContext + +CommandT = TypeVar("CommandT", contravariant=True) + + +@dataclass(frozen=True) +class ControllerStep: + """One robot decision; completion consumes no physical interval.""" + + command: RobotCommand | None = None + complete: bool = False + metrics: dict[str, float] = field(default_factory=dict) + error: str | None = None + policy_tick: bool = False + trace_bytes: bytes = b"" + latency_ns: dict[str, int] = field(default_factory=dict) + success: bool = True + + +class Controller(Protocol[CommandT]): + """Map robot state and command input to robot commands, including composite stages.""" + + def reset(self, state: RobotState) -> None: ... + + def step(self, state: RobotState, context: ControlContext[CommandT]) -> ControllerStep: ... + + def request_stop(self) -> None: ... diff --git a/motrix_deploy/src/motrix_deploy/runtime/result.py b/motrix_deploy/src/motrix_deploy/runtime/result.py index 7afd4fce..197f76c4 100644 --- a/motrix_deploy/src/motrix_deploy/runtime/result.py +++ b/motrix_deploy/src/motrix_deploy/runtime/result.py @@ -25,9 +25,7 @@ class RolloutResult: success: bool exit_reason: str completed_steps: int - simulation_time_s: float wall_time_s: float - real_time_factor: float overrun_count: int trace_sha256: str error: str | None = None @@ -37,6 +35,14 @@ def to_dict(self) -> dict[str, Any]: return asdict(self) +@dataclass(frozen=True, kw_only=True) +class SimulationRolloutResult(RolloutResult): + """Simulation outcome with model-only physical time, excluding readiness and damping.""" + + policy_simulation_time_s: float + real_time_factor: float + + class LatencyRecorder: """Collect latency samples and produce stable summaries.""" diff --git a/motrix_deploy/src/motrix_deploy/task.py b/motrix_deploy/src/motrix_deploy/task.py index a6576402..230756fa 100644 --- a/motrix_deploy/src/motrix_deploy/task.py +++ b/motrix_deploy/src/motrix_deploy/task.py @@ -1,61 +1,54 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Runtime contract implemented by concrete deployment-task packages.""" +"""Complete task control entry point and installed task discovery.""" from abc import ABC, abstractmethod from importlib import metadata from typing import Any, ClassVar, Generic, TypeVar from motrix_deploy.artifact.schema import TaskSpec -from motrix_deploy.contracts import FloatArray, RobotCommand, RobotSpec, RobotState -from motrix_deploy.runtime.context import PolicyContext +from motrix_deploy.contracts import RobotSpec, RobotState +from motrix_deploy.policy import Policy +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy.runtime.lifecycle import ControllerStep CommandT = TypeVar("CommandT") class DeployTask(ABC, Generic[CommandT]): - """Own task-specific observation, action, command, and termination semantics. - - 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. - """ + """Implement Controller and own task phases, without robot handles or scheduling.""" 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.""" + @classmethod + def default_duration_s(cls, spec: TaskSpec) -> float | None: + """Artifact-owned playback budget, if no CLI duration is supplied.""" return None @abstractmethod - def reset(self, state: RobotState, context: PolicyContext[CommandT]) -> None: - """Reset episode-local preprocessing and action state.""" + def reset(self, state: RobotState) -> None: ... @abstractmethod - def build_observation(self, state: RobotState, context: PolicyContext[CommandT]) -> FloatArray: - """Build one flat policy observation.""" + def step(self, state: RobotState, context: ControlContext[CommandT]) -> ControllerStep: ... @abstractmethod - def process_action(self, action: FloatArray) -> RobotCommand: - """Convert a policy action into a servo or torque command supported by the backend.""" + def request_stop(self) -> None: ... @abstractmethod - def validate_command(self, command: CommandT) -> None: - """Validate one external high-level command before using it.""" + def validate_command(self, command: CommandT | None) -> None: + """Validate external input at the application assembly boundary.""" TASK_ENTRY_POINT_GROUP = "motrix_deploy.tasks" def available_tasks() -> tuple[str, ...]: - """List installed task plugins without importing their implementations.""" + """List installed tasks without importing their implementations.""" return tuple(sorted({entry.name for entry in metadata.entry_points(group=TASK_ENTRY_POINT_GROUP)})) 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: @@ -65,13 +58,19 @@ def load_task_type(name: str) -> type[DeployTask[Any]]: 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") + raise TypeError(f"Deployment task plugin {name} must provide a DeployTask type") return task_type -def create_task(spec: TaskSpec, robot: RobotSpec) -> DeployTask[Any]: - """Instantiate the installed task class selected by an artifact.""" - return load_task_type(spec.name)(spec, robot) +def create_task( + spec: TaskSpec, + robot: RobotSpec, + policy: Policy, + steps: int | None = None, + artifact: Any | None = None, +) -> DeployTask[Any]: + """Construct a task; ``artifact`` supplies file payloads for tasks that reference them.""" + return load_task_type(spec.name)(spec, robot, policy, steps=steps, artifact=artifact) -__all__ = ["DeployTask", "create_task", "available_tasks"] +__all__ = ["DeployTask", "create_task", "available_tasks", "load_task_type"] diff --git a/motrix_deploy/tests/controller_helpers.py b/motrix_deploy/tests/controller_helpers.py new file mode 100644 index 00000000..4a8d24a5 --- /dev/null +++ b/motrix_deploy/tests/controller_helpers.py @@ -0,0 +1,139 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Reusable policy, policy I/O and bound hardware runtime helpers for tests.""" + +import numpy as np + +from motrix_deploy.artifact import DeploymentManifest +from motrix_deploy.contracts import ( + FloatArray, + JointControlMode, + JointServoCommand, + JointTorqueCommand, + RobotCommand, + RobotState, + float32_array, +) +from motrix_deploy.policy import Policy +from motrix_deploy.policy.processing import PolicyProcessor +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy.runtime.control import ControlSession, PolicyController +from motrix_deploy.runtime.hardware import HardwareRuntime +from motrix_deploy.runtime.scheduler import FixedStepScheduler +from motrix_env_core.input import ConstantPlanarVelocityBinding, PlanarVelocityCommand + + +class ConstantPolicy(Policy): + def __init__(self, input_size: int, action: FloatArray) -> None: + self._input_size = input_size + self._action = float32_array(action, path="test.action", shape=(action.size,)) + self.observations: list[FloatArray] = [] + + def infer(self, observation: FloatArray) -> FloatArray: + value = float32_array(observation, path="test.observation", shape=(self._input_size,)) + self.observations.append(value) + return self._action + + +class _TestPolicyProcessor(PolicyProcessor[PlanarVelocityCommand]): + def __init__(self, manifest: DeploymentManifest) -> None: + self.robot = manifest.robot + self._previous_action = np.zeros(self.robot.joint_count, dtype=np.float32) + + def reset(self, state: RobotState, context: ControlContext[PlanarVelocityCommand]) -> None: + del state, context + self._previous_action = np.zeros(2, dtype=np.float32) + + def build_observation(self, state: RobotState, context: ControlContext[PlanarVelocityCommand]) -> FloatArray: + del context + return np.concatenate((state.joint_position - self.robot.default_joint_position, state.joint_velocity)).astype( + np.float32 + ) + + 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 JointServoCommand( + joint_position=self.robot.default_joint_position + np.float32(0.25) * executed, + joint_velocity=zeros, + feedforward_torque=zeros, + kp=np.full(2, 35.0, dtype=np.float32), + kd=np.full(2, 0.5, dtype=np.float32), + ) + + def check_termination(self, state: RobotState) -> str | None: + return None + + def validate_command(self, command: PlanarVelocityCommand | None) -> None: + if not isinstance(command, PlanarVelocityCommand) or command.batch_size != 1: + raise ValueError("expected one planar velocity command") + + +class _TorquePolicyProcessor(_TestPolicyProcessor): + def process_action(self, action: FloatArray) -> JointTorqueCommand: + return JointTorqueCommand(np.array(action, dtype=np.float32, copy=True)) + + +class _AlternatingPolicyProcessor(_TestPolicyProcessor): + 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)) + + +class _StatefulPolicyProcessor(_TestPolicyProcessor): + def __init__(self, manifest: DeploymentManifest) -> None: + super().__init__(manifest) + self.contexts: list[ControlContext[PlanarVelocityCommand]] = [] + self.reset_contexts: list[ControlContext[PlanarVelocityCommand]] = [] + + def reset(self, state: RobotState, context: ControlContext[PlanarVelocityCommand]) -> None: + super().reset(state, context) + self.reset_contexts.append(context) + + def build_observation(self, state: RobotState, context: ControlContext[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) + + +def runtime_for(manifest, backend, *, steps=3, task=None, model=None, binding=None, scheduler=None): + task = task or _TestPolicyProcessor(manifest) + model = model or ConstantPolicy(4, np.array([0.4, -0.4], np.float32)) + controller = PolicyController(task, model, steps=steps) + runtime = HardwareRuntime(backend, scheduler=scheduler or FixedStepScheduler(manifest.control.period_s)) + session = ControlSession( + controller=controller, + robot=backend, + command_binding=binding or ConstantPlanarVelocityBinding((0.5, 0, 0)), + period_s=manifest.control.period_s, + state_timeout_s=manifest.control.state_timeout_s, + ) + runtime.bind_session(session) + return runtime, controller diff --git a/motrix_deploy/tests/fake_robot.py b/motrix_deploy/tests/fake_robot.py index d9ea05b0..61981851 100644 --- a/motrix_deploy/tests/fake_robot.py +++ b/motrix_deploy/tests/fake_robot.py @@ -148,6 +148,5 @@ def _state(self) -> RobotState: base_orientation_xyzw=np.array([0.0, 0.0, 0.0, 1.0], dtype=np.float32), base_angular_velocity=zeros, base_linear_acceleration=zeros, - base_position=np.array([0.0, 0.0, 0.3], dtype=np.float32), base_linear_velocity=zeros, ) diff --git a/motrix_deploy/tests/task_specs.py b/motrix_deploy/tests/task_specs.py index 222aca15..5ba61a9c 100644 --- a/motrix_deploy/tests/task_specs.py +++ b/motrix_deploy/tests/task_specs.py @@ -6,6 +6,7 @@ from typing import ClassVar from motrix_deploy.artifact.schema import TaskSpec +from motrix_deploy.runtime.lifecycle import ControllerStep from motrix_deploy.task import DeployTask @@ -16,18 +17,25 @@ class TestTaskSpec(TaskSpec): class TestDeployTask(DeployTask[object]): spec_type = TestTaskSpec - def __init__(self, spec, robot): + def __init__(self, spec, robot, policy, steps=None, artifact=None): self.spec = spec self.robot = robot + self.policy = policy + self.steps = steps + self.states = [] + self.stop_requested = False - def reset(self, state, context): - pass + def reset(self, state): + self.states = [state] + self.stop_requested = False - def build_observation(self, state, context): - raise NotImplementedError + def step(self, state, context): + self.states.append(state) + return ControllerStep(complete=True) - def process_action(self, action): - raise NotImplementedError + def request_stop(self): + self.stop_requested = True def validate_command(self, command): - pass + if command is not None: + raise ValueError("fixture task accepts no external input") diff --git a/motrix_deploy/tests/test_artifact.py b/motrix_deploy/tests/test_artifact.py index b892eb88..db084043 100644 --- a/motrix_deploy/tests/test_artifact.py +++ b/motrix_deploy/tests/test_artifact.py @@ -12,7 +12,14 @@ import pytest -from motrix_deploy.artifact import ControlSpec, DeploymentManifest, inspect_artifact, read_artifact, write_artifact +from motrix_deploy.artifact import ( + ControlSpec, + DeploymentManifest, + inspect_artifact, + read_artifact, + sha256_bytes, + write_artifact, +) from motrix_deploy.contracts import JointControlMode from motrix_deploy.errors import ArtifactError, ValidationError @@ -36,6 +43,32 @@ def test_artifact_round_trip_and_inspect( assert summary["policy"]["input"] == {"name": "observation", "shape": [1, 4], "dtype": "float32"} +def test_task_payload_round_trip_and_tamper_rejection( + tmp_path: Path, + manifest_factory: Callable[[], DeploymentManifest], +) -> None: + import dataclasses + + from motrix_deploy.artifact.schema import PayloadSpec + + payload = b"motion-clip-npz-bytes" + manifest = dataclasses.replace( + manifest_factory(), + payloads=(PayloadSpec(path="payloads/motion.npz", sha256=sha256_bytes(payload)),), + ) + output = tmp_path / "fixture.deploy" + artifact = write_artifact(output, manifest, {"policy/model.onnx": POLICY_BYTES, "payloads/motion.npz": payload}) + assert (output / "payloads" / "motion.npz").read_bytes() == payload + assert artifact.manifest.payloads[0].path == "payloads/motion.npz" + # A modified task payload is rejected by its checksum. + (output / "payloads" / "motion.npz").write_bytes(b"tampered") + with pytest.raises(ArtifactError, match="payload sha256 mismatch"): + read_artifact(output) + # Missing and unexpected payload keys are rejected at the write boundary. + with pytest.raises(ArtifactError, match="payload keys must be exactly"): + write_artifact(tmp_path / "short.deploy", manifest, {"policy/model.onnx": POLICY_BYTES}) + + def test_cli_inspect_outputs_json(tmp_path: Path, manifest_factory: Callable[[], DeploymentManifest]) -> None: output = tmp_path / "fixture.deploy" write_artifact(output, manifest_factory(), {"policy/model.onnx": POLICY_BYTES}) diff --git a/motrix_deploy/tests/test_policy.py b/motrix_deploy/tests/test_policy.py index d39846ba..21f58456 100644 --- a/motrix_deploy/tests/test_policy.py +++ b/motrix_deploy/tests/test_policy.py @@ -12,11 +12,11 @@ from motrix_deploy.contracts import TensorSpec from motrix_deploy.errors import ValidationError -from motrix_deploy.policy import NoOpPolicyRuntime, OnnxPolicyRuntime, PolicyRuntime +from motrix_deploy.policy import NoOpPolicy, OnnxPolicy, Policy def test_decision_runtime_only_requires_infer() -> None: - class Planner(PolicyRuntime): + class Planner(Policy): def infer(self, observation): return observation[::-1].copy() @@ -26,7 +26,7 @@ def infer(self, observation): def test_noop_returns_independent_zero_actions_and_ignores_observation() -> None: - runtime = NoOpPolicyRuntime(3) + runtime = NoOpPolicy(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)) @@ -35,12 +35,6 @@ def test_noop_returns_independent_zero_actions_and_ignores_observation() -> None 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( path: Path, *, @@ -72,7 +66,7 @@ def _write_linear_model( def test_onnx_policy_validates_io_and_runs_float32(tmp_path: Path) -> None: path = tmp_path / "policy.onnx" _write_linear_model(path) - runtime = OnnxPolicyRuntime( + runtime = OnnxPolicy( path, TensorSpec(name="observation", shape=(1, 4)), TensorSpec(name="action", shape=(1, 2)), @@ -87,7 +81,7 @@ def test_onnx_policy_validates_io_and_runs_float32(tmp_path: Path) -> None: def test_onnx_policy_accepts_dynamic_model_batch_for_single_sample_runtime(tmp_path: Path) -> None: path = tmp_path / "policy.onnx" _write_linear_model(path, dynamic_batch=True) - runtime = OnnxPolicyRuntime( + runtime = OnnxPolicy( path, TensorSpec(name="observation", shape=(1, 4)), TensorSpec(name="action", shape=(1, 2)), @@ -103,7 +97,7 @@ def test_onnx_policy_rejects_manifest_name_mismatch(tmp_path: Path) -> None: _write_linear_model(path, output_name="actual_action") with pytest.raises(ValidationError, match="policy.output.name"): - OnnxPolicyRuntime( + OnnxPolicy( path, TensorSpec(name="observation", shape=(1, 4)), TensorSpec(name="action", shape=(1, 2)), @@ -113,7 +107,7 @@ def test_onnx_policy_rejects_manifest_name_mismatch(tmp_path: Path) -> None: def test_onnx_policy_rejects_non_finite_input(tmp_path: Path) -> None: path = tmp_path / "policy.onnx" _write_linear_model(path) - runtime = OnnxPolicyRuntime( + runtime = OnnxPolicy( path, TensorSpec(name="observation", shape=(1, 4)), TensorSpec(name="action", shape=(1, 2)), @@ -126,7 +120,7 @@ def test_onnx_policy_rejects_non_finite_input(tmp_path: Path) -> None: def test_onnx_policy_rejects_non_finite_output(tmp_path: Path) -> None: path = tmp_path / "policy.onnx" _write_linear_model(path, nan_output=True) - runtime = OnnxPolicyRuntime( + runtime = OnnxPolicy( path, TensorSpec(name="observation", shape=(1, 4)), TensorSpec(name="action", shape=(1, 2)), diff --git a/motrix_deploy/tests/test_policy_controller.py b/motrix_deploy/tests/test_policy_controller.py new file mode 100644 index 00000000..c588276d --- /dev/null +++ b/motrix_deploy/tests/test_policy_controller.py @@ -0,0 +1,104 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""PolicyController computes commands from measurements without backend side effects.""" + +import numpy as np +import pytest +from controller_helpers import ConstantPolicy, _StatefulPolicy, _StatefulPolicyProcessor, _TestPolicyProcessor +from fake_robot import FakeRobotInterface + +from motrix_deploy.errors import ControlFailure +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy.runtime.control import PolicyController +from motrix_env_core.input import PlanarVelocityCommand + + +def context(step=0, time_s=0.0): + return ControlContext(step, time_s, PlanarVelocityCommand(np.zeros((1, 3), dtype=np.float32)), dt_s=0.02) + + +def test_policy_controller_uses_supplied_state_and_returns_command(manifest_factory): + manifest = manifest_factory() + robot = FakeRobotInterface(manifest.robot) + task = _TestPolicyProcessor(manifest) + model = ConstantPolicy(4, np.zeros(2, np.float32)) + controller = PolicyController(task, model, steps=1) + assert controller.policy_io is task + assert controller.policy is model + robot.open() + state = robot.read_state(0.1) + events = list(robot.events) + seen = [] + observe = task.build_observation + + def observation(measurement, inputs): + seen.append((measurement, inputs)) + return observe(measurement, inputs) + + task.build_observation = observation + controller.reset(state) + decision = controller.step(state, context()) + assert decision.command is not None and not decision.complete + assert seen[0][0] is state + assert robot.events == events + assert controller.completed_steps == 1 + assert decision.policy_tick + assert decision.trace_bytes + assert decision.latency_ns["inference"] >= 0 + final = controller.step(state, context(1, 0.02)) + assert final.complete and final.command is None + assert controller.completed_steps == 1 + + +def test_model_failure_propagates_without_backend_side_effects(manifest_factory): + manifest = manifest_factory() + robot = FakeRobotInterface(manifest.robot) + model = ConstantPolicy(4, np.zeros(2, np.float32)) + controller = PolicyController(_TestPolicyProcessor(manifest), model) + robot.open() + state = robot.read_state(0.1) + controller.reset(state) + events = list(robot.events) + + def infer(observation): + raise RuntimeError("controller failed") + + model.infer = infer + with pytest.raises(ControlFailure, match="controller failed") as error: + controller.step(state, context()) + assert error.value.reason == "policy_error" + assert controller.completed_steps == 0 + assert robot.events == events + + +def test_policy_controller_owns_local_count_clock_and_history_reset(manifest_factory): + manifest = manifest_factory() + robot = FakeRobotInterface(manifest.robot) + robot.open() + state = robot.read_state(0.1) + task = _StatefulPolicyProcessor(manifest) + model = _StatefulPolicy() + controller = PolicyController(task, model, steps=2) + controller.reset(state) + controller.step(state, context(42, 3.0)) + controller.step(state, context(43, 3.07)) + assert model.reset_count == 1 + assert len(task.reset_contexts) == 1 + assert [inputs.step for inputs in task.contexts] == [0, 1] + assert [inputs.elapsed_time_s for inputs in task.contexts] == pytest.approx([0, 0.07]) + assert controller.step(state, context(44, 3.09)).complete + + +def test_requested_stop_completes_without_another_inference(manifest_factory): + manifest = manifest_factory() + robot = FakeRobotInterface(manifest.robot) + robot.open() + state = robot.read_state(0.1) + model = ConstantPolicy(4, np.zeros(2, np.float32)) + controller = PolicyController(_TestPolicyProcessor(manifest), model) + controller.reset(state) + assert controller.step(state, context()).command is not None + controller.request_stop() + assert controller.step(state, context(1, 0.02)).complete + assert len(model.observations) == 1 diff --git a/motrix_deploy/tests/test_runtime.py b/motrix_deploy/tests/test_runtime.py index 289ce139..3b282e91 100644 --- a/motrix_deploy/tests/test_runtime.py +++ b/motrix_deploy/tests/test_runtime.py @@ -1,905 +1,479 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Unified control loop and fake backend contract tests.""" +"""Robot controller execution and runtime safety boundary contracts.""" -from collections.abc import Callable +import time +from dataclasses import replace import numpy as np import pytest +from controller_helpers import ( + ConstantPolicy, + _AlternatingPolicyProcessor, + _StatefulPolicy, + _StatefulPolicyProcessor, + _TestPolicyProcessor, + _TorquePolicyProcessor, + runtime_for, +) from fake_robot import FakeRobotInterface -from motrix_deploy.artifact import DeploymentManifest 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 ( - ControlSession, - FixedStepScheduler, - HardwareRuntime, - PolicyContext, - RealtimeScheduler, -) -from motrix_deploy.task import DeployTask -from motrix_env_core.input import CommandBinding, ConstantPlanarVelocityBinding, PlanarVelocityCommand +from motrix_deploy.runtime.control import ControlSession, PolicyController +from motrix_deploy.runtime.hardware import HardwareRuntime +from motrix_deploy.runtime.lifecycle import ControllerStep +from motrix_deploy.runtime.result import LatencySummary, RolloutResult +from motrix_deploy.runtime.scheduler import FixedStepScheduler, RealtimeScheduler +from motrix_env_core.input import CommandBinding, PlanarVelocityCommand -class ConstantPolicy(PolicyRuntime): - def __init__(self, input_size: int, action: FloatArray) -> None: - self._input_size = input_size - self._action = float32_array(action, path="test.action", shape=(action.size,)) - self.observations: list[FloatArray] = [] +class _FailingBinding(CommandBinding[PlanarVelocityCommand]): + def read_command(self, *, batch_size: int = 1) -> PlanarVelocityCommand: + del batch_size + raise RuntimeError("input disconnected") - def infer(self, observation: FloatArray) -> FloatArray: - value = float32_array(observation, path="test.observation", shape=(self._input_size,)) - self.observations.append(value) - return self._action +class _TrackingBinding(CommandBinding[PlanarVelocityCommand]): + def __init__(self) -> None: + self.batch_sizes: list[int] = [] -class _TestDeployTask(DeployTask[PlanarVelocityCommand]): - def __init__(self, manifest: DeploymentManifest) -> None: - self.robot = manifest.robot - self._previous_action = np.zeros(self.robot.joint_count, dtype=np.float32) + def read_command(self, *, batch_size: int = 1) -> PlanarVelocityCommand: + self.batch_sizes.append(batch_size) + return PlanarVelocityCommand(np.zeros((batch_size, 3), dtype=np.float32)) - def reset(self, state: RobotState, context: PolicyContext[PlanarVelocityCommand]) -> None: - del state, context - self._previous_action = np.zeros(2, dtype=np.float32) - def build_observation(self, state: RobotState, context: PolicyContext[PlanarVelocityCommand]) -> FloatArray: - del context - return np.concatenate((state.joint_position - self.robot.default_joint_position, state.joint_velocity)).astype( - np.float32 - ) +class _InterruptingBinding(CommandBinding[PlanarVelocityCommand]): + def __init__(self, interrupt_at: int) -> None: + self._interrupt_at = interrupt_at + self.read_count = 0 - 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 JointServoCommand( - joint_position=self.robot.default_joint_position + np.float32(0.25) * executed, - joint_velocity=zeros, - feedforward_torque=zeros, - kp=np.full(2, 35.0, dtype=np.float32), - kd=np.full(2, 0.5, dtype=np.float32), + def read_command(self, *, batch_size: int = 1) -> PlanarVelocityCommand: + if self.read_count == self._interrupt_at: + raise KeyboardInterrupt + self.read_count += 1 + return PlanarVelocityCommand(np.zeros((batch_size, 3), dtype=np.float32)) + + +class EnablingBackend(FakeRobotInterface): + 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_servo",), + state_fields=self._capabilities.state_fields, + max_command_rate_hz=1.0 / sample_period_s, + requires_enable=True, ) + self.enabled = False - def validate_command(self, command: PlanarVelocityCommand) -> None: - if not isinstance(command, PlanarVelocityCommand) or command.batch_size != 1: - raise ValueError("expected one planar velocity command") + def enable(self) -> None: + assert "write" not in self.events + self.events.append("enable") + self.enabled = True + + def write_command(self, command: RobotCommand) -> None: + assert self.enabled + super().write_command(command) -class _TorqueDeployTask(_TestDeployTask): - def process_action(self, action: FloatArray) -> JointTorqueCommand: - return JointTorqueCommand(np.array(action, dtype=np.float32, copy=True)) +class LieDownBackend(FakeRobotInterface): + def write_command(self, command: RobotCommand) -> None: + del command + raise LieDownRequestedError("remote B requested lie-down shutdown") +@pytest.mark.parametrize("task_type", [_TestPolicyProcessor, _TorquePolicyProcessor, _AlternatingPolicyProcessor]) @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: +def test_runtime_executes_deterministic_controller_commands(manifest_factory, task_type, requires_enable): manifest = manifest_factory() - traces = [] + runs = [] 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 + backend._capabilities = replace( + backend.capabilities, control_modes=(JointControlMode.SERVO, JointControlMode.TORQUE) + ) + runtime, controller = runtime_for(manifest, backend, task=task_type(manifest), steps=4) + result = runtime.run() + assert result.success and result.completed_steps == 4 + assert len(backend.commands) == 4 + assert backend.events[-2:] == ["stop", "close"] 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)) + if task_type is _TorquePolicyProcessor: + assert all(command.mode is JointControlMode.TORQUE for command in backend.commands) + np.testing.assert_array_equal(backend._position, manifest.robot.default_joint_position) + if task_type is _AlternatingPolicyProcessor: + assert [command.mode for command in backend.commands] == [ + JointControlMode.SERVO, + JointControlMode.TORQUE, + ] * 2 + runs.append((result, controller, backend)) + assert runs[0][0].trace_sha256 == runs[1][0].trace_sha256 + for first, second in zip(runs[0][1].policy.observations, runs[1][1].policy.observations, strict=True): + np.testing.assert_array_equal(first, second) -@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: +@pytest.mark.parametrize("task_type,completed", [(_TorquePolicyProcessor, 0), (_AlternatingPolicyProcessor, 1)]) +def test_unsupported_mode_is_rejected_before_backend_write(manifest_factory, task_type, completed): 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.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, - ) - - -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) - + backend._capabilities = replace(backend.capabilities, control_modes=(JointControlMode.SERVO,)) + runtime, _ = runtime_for(manifest, backend, task=task_type(manifest)) + result = runtime.run() 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 + assert result.completed_steps == completed + assert len(backend.commands) == completed + assert "control mode" in result.error + assert backend.events[-2:] == ["stop", "close"] @pytest.mark.parametrize("response", [1.0, 0.5]) -def test_control_loop_runs_with_different_robot_responses( - manifest_factory: Callable[[], DeploymentManifest], - response: float, -) -> None: +def test_runtime_accepts_different_measured_robot_responses(manifest_factory, response): manifest = manifest_factory() backend = FakeRobotInterface(manifest.robot, response=response) - - runtime, control = _loop(manifest, backend) - result = runtime.run(control, steps=5) - + runtime, _ = runtime_for(manifest, backend, steps=5) + result = runtime.run() assert result.success - assert result.exit_reason == "completed" + assert isinstance(result, RolloutResult) assert result.completed_steps == 5 - assert result.simulation_time_s == pytest.approx(0.1) + assert result.wall_time_s >= 0 + report = result.to_dict() + assert report["success"] and report["completed_steps"] == 5 + assert report["wall_time_s"] == result.wall_time_s + assert isinstance(result.latency["read"], LatencySummary) + assert report["latency"]["read"]["mean_ms"] == result.latency["read"].mean_ms assert len(backend.commands) == 5 - assert backend.events[-2:] == ["stop", "close"] - assert set(result.latency) == {"input", "read", "observation", "inference", "action", "write", "loop"} + assert set(result.latency) >= {"input", "read", "observation", "inference", "action", "write", "loop"} -def test_fixed_step_rollout_is_deterministic(manifest_factory: Callable[[], DeploymentManifest]) -> None: - manifest = manifest_factory() - first_backend = FakeRobotInterface(manifest.robot) - second_backend = FakeRobotInterface(manifest.robot) - - 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, - second.exit_reason, - second.completed_steps, - ) - for first_command, second_command in zip(first_backend.commands, second_backend.commands, strict=True): - np.testing.assert_array_equal(first_command.joint_position, second_command.joint_position) - assert isinstance(first_loop.policy, ConstantPolicy) - assert isinstance(second_loop.policy, ConstantPolicy) - for first_observation, second_observation in zip( - first_loop.policy.observations, - second_loop.policy.observations, - strict=True, - ): - np.testing.assert_array_equal(first_observation, second_observation) - - -class _FailingBinding(CommandBinding[PlanarVelocityCommand]): - def read_command(self, *, batch_size: int = 1) -> PlanarVelocityCommand: - del batch_size - raise RuntimeError("input disconnected") - - -class _TrackingBinding(CommandBinding[PlanarVelocityCommand]): - def __init__(self) -> None: - self.batch_sizes: list[int] = [] - - def read_command(self, *, batch_size: int = 1) -> PlanarVelocityCommand: - self.batch_sizes.append(batch_size) - return PlanarVelocityCommand(np.zeros((batch_size, 3), dtype=np.float32)) - - -class _InterruptingBinding(CommandBinding[PlanarVelocityCommand]): - def __init__(self, interrupt_at: int) -> None: - self._interrupt_at = interrupt_at - self.read_count = 0 - - def read_command(self, *, batch_size: int = 1) -> PlanarVelocityCommand: - if self.read_count == self._interrupt_at: - raise KeyboardInterrupt - self.read_count += 1 - return PlanarVelocityCommand(np.zeros((batch_size, 3), dtype=np.float32)) - - -def test_control_loop_reads_one_singleton_command_batch_per_tick( - manifest_factory: Callable[[], DeploymentManifest], -) -> None: +def test_runtime_reads_singleton_commands_for_policy_ticks(manifest_factory): manifest = manifest_factory() binding = _TrackingBinding() - runtime, loop = _loop(manifest, FakeRobotInterface(manifest.robot), command_binding=binding) - loop.policy = ConstantPolicy(manifest.policy.input.shape[1], np.zeros(2, dtype=np.float32)) + runtime, _ = runtime_for(manifest, FakeRobotInterface(manifest.robot), binding=binding) + assert runtime.run().success + assert all(size == 1 for size in binding.batch_sizes) + assert len(binding.batch_sizes) >= 3 - result = runtime.run(loop, steps=3) - assert result.success - assert binding.batch_sizes == [1, 1, 1] - - -def test_control_loop_runs_without_a_step_bound_until_interrupted( - manifest_factory: Callable[[], DeploymentManifest], -) -> None: +def test_unbounded_controller_runs_until_input_interrupt(manifest_factory): manifest = manifest_factory() - binding = _InterruptingBinding(interrupt_at=3) - 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 = runtime.run(loop) - + runtime, _ = runtime_for(manifest, FakeRobotInterface(manifest.robot), steps=None, binding=_InterruptingBinding(3)) + result = runtime.run() assert not result.success assert result.exit_reason == "interrupted" assert result.completed_steps == 3 -def test_command_binding_failure_returns_input_error(manifest_factory: Callable[[], DeploymentManifest]) -> None: +def test_input_failure_is_structured_and_cleans_backend(manifest_factory): manifest = manifest_factory() 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 = runtime.run(loop, steps=1) - - assert not result.success + runtime, _ = runtime_for(manifest, backend, binding=_FailingBinding()) + result = runtime.run() assert result.exit_reason == "input_error" assert result.error == "input disconnected" assert backend.events[-2:] == ["stop", "close"] @pytest.mark.parametrize( - ("backend_kwargs", "reason", "completed_steps"), + "options,reason,count", [ ({"fail_read_at": 0}, "state_timeout", 0), ({"unhealthy_at": 1}, "backend_unhealthy", 0), ({"fail_write_at": 2}, "backend_write_error", 2), ], ) -def test_backend_failures_return_structured_results_and_cleanup( - manifest_factory: Callable[[], DeploymentManifest], - backend_kwargs: dict[str, int], - reason: str, - completed_steps: int, -) -> None: +def test_backend_failure_preserves_result_and_cleanup(manifest_factory, options, reason, count): manifest = manifest_factory() - backend = FakeRobotInterface(manifest.robot, **backend_kwargs) - - runtime, control = _loop(manifest, backend) - result = runtime.run(control, steps=5) - + backend = FakeRobotInterface(manifest.robot, **options) + runtime, _ = runtime_for(manifest, backend, steps=5) + result = runtime.run() assert not result.success assert result.exit_reason == reason - assert result.completed_steps == completed_steps + assert result.completed_steps == count assert backend.events[-2:] == ["stop", "close"] -def test_backend_open_validation_failure_cleans_up_before_first_command( - manifest_factory: Callable[[], DeploymentManifest], -) -> None: - class InvalidRobot(FakeRobotInterface): - def open(self) -> None: - self.events.append("open") - raise ValidationError("backend.joint_names", "matching robot joints", "missing_joint") +@pytest.mark.parametrize( + "kind,reason", + [ + ("invalid", "invalid_state"), + ("stale", "state_timeout"), + ("backwards", "invalid_state"), + ], +) +def test_runtime_validates_state_contract_and_freshness(manifest_factory, kind, reason): + manifest = manifest_factory() + + class StateBackend(FakeRobotInterface): + def read_state(self, timeout_s): + state = super().read_state(timeout_s) + if kind == "invalid": + return None + if kind == "stale": + return replace(state, receive_time_ns=time.monotonic_ns() - 1_000_000_000) + if self._read_count > 1: + return replace(state, sample_time_ns=0) + return replace(state, sample_time_ns=10) + + backend = StateBackend(manifest.robot) + runtime, _ = runtime_for(manifest, backend) + result = runtime.run() + assert not result.success and result.exit_reason == reason + assert backend.events[-2:] == ["stop", "close"] + +def test_open_validation_failure_cleans_before_first_command(manifest_factory): manifest = manifest_factory() - backend = InvalidRobot(manifest.robot) - runtime, control = _loop(manifest, backend) - result = runtime.run(control, steps=2) + class InvalidRobot(FakeRobotInterface): + def open(self): + self.events.append("open") + raise ValidationError("backend.joint_names", "matching joints", "missing") + backend = InvalidRobot(manifest.robot) + runtime, _ = runtime_for(manifest, backend) + result = runtime.run() assert result.exit_reason == "validation_error" assert result.completed_steps == 0 - assert backend.commands == [] assert backend.events == ["open", "stop", "close"] -class InterruptingBackend(FakeRobotInterface): - def read_state(self, timeout_s: float): - del timeout_s - raise KeyboardInterrupt - - -def test_user_interrupt_returns_result_and_cleans_up(manifest_factory: Callable[[], DeploymentManifest]) -> None: +@pytest.mark.parametrize( + "error,reason", [(KeyboardInterrupt(), "interrupted"), (LieDownRequestedError("lie down"), "lie_down")] +) +def test_runtime_interrupts_reach_backend_cleanup(manifest_factory, error, reason): manifest = manifest_factory() - backend = InterruptingBackend(manifest.robot) - runtime, control = _loop(manifest, backend) - result = runtime.run(control, steps=2) + class InterruptedRobot(FakeRobotInterface): + def read_state(self, timeout_s): + raise error - assert result.exit_reason == "interrupted" + backend = InterruptedRobot(manifest.robot) + runtime, _ = runtime_for(manifest, backend) + assert runtime.run().exit_reason == reason assert backend.events[-2:] == ["stop", "close"] -class InvalidStateBackend(FakeRobotInterface): - def read_state(self, timeout_s: float): - del timeout_s - return None - - -def test_invalid_state_fails_and_lifecycle_cleanup_is_idempotent( - manifest_factory: Callable[[], DeploymentManifest], -) -> None: +def test_host_running_check_failure_stops_before_command_and_cleans_up(manifest_factory, monkeypatch): manifest = manifest_factory() - backend = InvalidStateBackend(manifest.robot) - - runtime, control = _loop(manifest, backend) - result = runtime.run(control, steps=2) - events_after_run = list(backend.events) - backend.stop() - backend.close() - - assert result.exit_reason == "invalid_state" - assert result.completed_steps == 0 - assert backend.events == events_after_run - - -def test_realtime_scheduler_uses_absolute_deadlines() -> None: - now = [1_000_000_000] - sleeps: list[float] = [] - - def clock_ns() -> int: - return now[0] - - def sleep(duration: float) -> None: - sleeps.append(duration) - now[0] += round(duration * 1e9) - - scheduler = RealtimeScheduler(0.02, clock_ns=clock_ns, sleep=sleep) - scheduler.reset() - scheduler.wait(1) - now[0] += 5_000_000 - scheduler.wait(2) - - assert sleeps == pytest.approx([0.02, 0.015]) - assert scheduler.elapsed_time_s(2) == pytest.approx(0.04) - - -class EnablingBackend(FakeRobotInterface): - 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_servo",), - state_fields=self._capabilities.state_fields, - max_command_rate_hz=1.0 / sample_period_s, - requires_enable=True, - ) - self.enabled = False - - def enable(self) -> None: - assert "write" not in self.events - self.events.append("enable") - self.enabled = True - - def write_command(self, command: RobotCommand) -> None: - assert self.enabled - super().write_command(command) + backend = FakeRobotInterface(manifest.robot) + runtime, _ = runtime_for(manifest, backend) + def reject_interval(): + raise RuntimeError("host stopped") -class LieDownBackend(FakeRobotInterface): - def write_command(self, command: RobotCommand) -> None: - del command - raise LieDownRequestedError("remote B requested lie-down shutdown") + monkeypatch.setattr(runtime, "_check_running", reject_interval) + result = runtime.run() + assert result.exit_reason == "backend_error" + assert result.error == "host stopped" + assert not backend.commands + assert backend.events[-2:] == ["stop", "close"] -def test_lie_down_request_returns_structured_result_and_cleans_up( - manifest_factory: Callable[[], DeploymentManifest], -) -> None: +@pytest.mark.parametrize("phase", ["reset", "wait"]) +def test_scheduler_failures_preserve_commands_and_cleanup(manifest_factory, phase): manifest = manifest_factory() - backend = LieDownBackend(manifest.robot) + backend = FakeRobotInterface(manifest.robot) + scheduler = FixedStepScheduler(manifest.control.period_s) - runtime, control = _loop(manifest, backend) - result = runtime.run(control, steps=1) + def fail(*args): + raise RuntimeError("scheduler failed") - assert not result.success - assert result.exit_reason == "lie_down" - assert result.completed_steps == 0 + setattr(scheduler, phase, fail) + runtime, _ = runtime_for(manifest, backend, scheduler=scheduler) + result = runtime.run() + assert result.exit_reason == "backend_error" + assert result.error == "scheduler failed" + assert len(backend.commands) == (1 if phase == "wait" else 0) 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: +@pytest.mark.parametrize("prior_failure", [False, True]) +def test_cleanup_error_preserves_prior_failure_and_attempts_close(manifest_factory, prior_failure): 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"] + backend = FakeRobotInterface(manifest.robot, fail_write_at=0 if prior_failure else None) + def fail_stop(): + backend.events.append("stop-failed") + raise RuntimeError("cleanup failed") -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"] + backend.stop = fail_stop + runtime, _ = runtime_for(manifest, backend, steps=1) + result = runtime.run() + assert result.exit_reason == ("backend_write_error" if prior_failure else "cleanup_error") + assert backend.events[-2:] == ["stop-failed", "close"] -def test_task_termination_hook_failure_has_task_error_and_cleanup( - manifest_factory: Callable[[], DeploymentManifest], -) -> None: +def test_finite_controller_validates_final_measured_state(manifest_factory): 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 == [] + task = _TestPolicyProcessor(manifest) + task.check_termination = lambda state: ( + "final state unsafe" if state.sample_time_ns >= 2 * backend._period_ns else None + ) + runtime, _ = runtime_for(manifest, backend, task=task, steps=2) + result = runtime.run() + assert not result.success and result.exit_reason == "task_terminated" + assert result.completed_steps == 2 + assert len(backend.commands) == 2 assert backend.events[-2:] == ["stop", "close"] -def test_required_enable_completes_before_first_policy_write( - manifest_factory: Callable[[], DeploymentManifest], -) -> None: +def test_controller_receives_post_enable_measurements_and_resets_history(manifest_factory): manifest = manifest_factory() backend = EnablingBackend(manifest.robot) - - runtime, control = _loop(manifest, backend) - result = runtime.run(control, steps=1) - + task = _StatefulPolicyProcessor(manifest) + model = _StatefulPolicy() + runtime, _ = runtime_for(manifest, backend, task=task, model=model) + result = runtime.run() assert result.success - assert backend.events[:6] == ["open", "read", "enable", "read", "read", "write"] + assert backend.events.index("enable") < backend.events.index("write") + assert model.reset_count >= 1 + assert [context.step for context in task.contexts] == [0, 1, 2] + assert [context.elapsed_time_s for context in task.contexts] == pytest.approx([0, 0.02, 0.04]) -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_realtime_scheduler_uses_absolute_deadlines(): + now = [1_000_000_000] + sleeps = [] + def sleep(duration): + sleeps.append(duration) + now[0] += round(duration * 1e9) -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 + scheduler = RealtimeScheduler(0.02, clock_ns=lambda: now[0], sleep=sleep) + scheduler.reset() + scheduler.wait(1) + now[0] += 5_000_000 + scheduler.wait(2) + assert sleeps == pytest.approx([0.02, 0.015]) + assert scheduler.elapsed_time_s(2) == pytest.approx(0.04) + now[0] += 25_000_000 + scheduler.wait(3) + assert scheduler.overrun_count == 1 + scheduler.reset() + assert scheduler.overrun_count == 0 -def test_failed_start_records_error_without_implicit_cleanup( - manifest_factory: Callable[[], DeploymentManifest], -) -> None: +@pytest.mark.parametrize( + "fault,reason", + [ + ("enable", "backend_enable_error"), + ("termination", "task_termination_error"), + ("observation", "invalid_observation"), + ("inference", "policy_error"), + ("action", "invalid_action"), + ], +) +def test_policy_and_enable_failures_keep_structured_error(manifest_factory, fault, reason): 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" + backend = EnablingBackend(manifest.robot) + task = _TestPolicyProcessor(manifest) + model = ConstantPolicy(4, np.zeros(2, np.float32)) + + def fail(*args): + raise RuntimeError("injected boundary failure") + + target, method = { + "enable": (backend, "enable"), + "termination": (task, "check_termination"), + "observation": (task, "build_observation"), + "inference": (model, "infer"), + "action": (task, "process_action"), + }[fault] + setattr(target, method, fail) + runtime, _ = runtime_for(manifest, backend, task=task, model=model) + result = runtime.run() + assert not result.success and result.exit_reason == reason + assert result.error == "injected boundary failure" + assert result.completed_steps == 0 + assert backend.commands == [] 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]] = [] +class DeterministicController: + """A deterministic controller that does not contain a model or task adapter.""" - def reset(self, state: RobotState, context: PolicyContext[PlanarVelocityCommand]) -> None: - super().reset(state, context) - self.reset_contexts.append(context) + def reset(self, state): + self.position = state.joint_position.copy() + self.calls = 0 + self.stop_requested = False - 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 + def request_stop(self): + self.stop_requested = True + def step(self, state, context): + self.command = context.command + if self.stop_requested or self.calls == 2: + return ControllerStep(complete=True) + self.calls += 1 + return ControllerStep(command=JointTorqueCommand(np.zeros_like(state.joint_position))) -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: +def test_missing_required_command_fails_before_inference_and_write(manifest_factory): 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 + backend = FakeRobotInterface(manifest.robot) + model = ConstantPolicy(4, np.zeros(2, np.float32)) + session = ControlSession( + robot=backend, + controller=PolicyController(_TestPolicyProcessor(manifest), model, steps=1), + period_s=0.02, + state_timeout_s=0.1, + ) + runtime = HardwareRuntime(backend, realtime=False) + runtime.bind_session(session) + result = runtime.run() + assert result.exit_reason == "input_error" + assert not model.observations + assert not backend.commands + assert backend.events[-2:] == ["stop", "close"] -@pytest.mark.parametrize("phase", ["reset", "wait"]) -def test_driver_scheduler_failure_is_recorded_and_cleans_up( - manifest_factory: Callable[[], DeploymentManifest], - phase: str, -) -> None: +def test_control_executes_model_free_controller_and_reports_generic_completion(manifest_factory): 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 + controller = DeterministicController() + session = ControlSession( + controller=controller, + robot=backend, + period_s=0.02, + state_timeout_s=0.1, + ) + runtime = HardwareRuntime(backend, realtime=False) + runtime.bind_session(session) + result = runtime.run() + assert result.success + assert result.completed_steps == 0 # Deterministic commands are not model ticks. + assert len(backend.commands) == 2 + assert controller.calls == 2 + assert controller.command is None 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: +def test_control_requested_stop_is_controller_owned_and_cleanup_is_idempotent(manifest_factory): manifest = manifest_factory() backend = FakeRobotInterface(manifest.robot) - session = _session(manifest, backend) + controller = DeterministicController() + session = ControlSession( + controller=controller, + robot=backend, + period_s=0.02, + state_timeout_s=0.1, + ) 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.request_stop() + assert not session.tick(elapsed_time_s=0.02) session.stop() + events = list(backend.events) 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"] + assert backend.events == events + assert session.result().success + assert len(backend.commands) == 1 diff --git a/motrix_deploy/tests/test_runtime_binding.py b/motrix_deploy/tests/test_runtime_binding.py index f64b75a6..74cef5f2 100644 --- a/motrix_deploy/tests/test_runtime_binding.py +++ b/motrix_deploy/tests/test_runtime_binding.py @@ -1,64 +1,102 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Runtime/control ownership binding contracts.""" +"""Runtime binds robot session; session binds a backend-independent controller.""" from types import SimpleNamespace +import numpy as np import pytest +from controller_helpers import ConstantPolicy, _TestPolicyProcessor 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.control import ControlSession, PolicyController from motrix_deploy.runtime.factory import create_hardware_runtime from motrix_deploy.runtime.hardware import HardwareRuntime -from motrix_deploy.runtime.scheduler import FixedStepScheduler +from motrix_env_core.input import ConstantPlanarVelocityBinding -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)) +def make_controller(manifest, steps=2): + return PolicyController(_TestPolicyProcessor(manifest), ConstantPolicy(4, np.zeros(2, np.float32)), steps=steps) - 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 bind(runtime, controller, period_s=0.02): + session = ControlSession( + controller=controller, + robot=runtime.robot, + command_binding=ConstantPlanarVelocityBinding((0, 0, 0)), + period_s=period_s, + state_timeout_s=0.1, + ) + runtime.bind_session(session) + return session -def test_binding_rejects_conflicting_robot_and_duplicate_control(manifest_factory): +def test_runtime_rejects_conflicting_session_binding(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): + runtime = HardwareRuntime(FakeRobotInterface(manifest.robot), realtime=False) + first = bind(runtime, make_controller(manifest)) + with pytest.raises(ValidationError, match="bound"): + bind(runtime, make_controller(manifest)) + assert runtime.session is first + + +def test_runtime_rejects_session_for_another_robot(manifest_factory): manifest = manifest_factory() - runtime = HardwareRuntime(FakeRobotInterface(manifest.robot)) + runtime = HardwareRuntime(FakeRobotInterface(manifest.robot), realtime=False) + foreign = ControlSession( + controller=make_controller(manifest), + robot=FakeRobotInterface(manifest.robot), + command_binding=ConstantPlanarVelocityBinding((0, 0, 0)), + period_s=0.02, + state_timeout_s=0.1, + ) + with pytest.raises(ValidationError, match="robot"): + runtime.bind_session(foreign) + assert runtime.session is None + + +def test_unbound_runtime_requires_session(manifest_factory): + runtime = HardwareRuntime(FakeRobotInterface(manifest_factory().robot), realtime=False) + with pytest.raises(ValidationError, match="session"): + runtime.run() + + +def test_session_supplies_its_execution_period_to_controller(manifest_factory): + manifest = manifest_factory() + controller = make_controller(manifest) + contexts = [] + step = controller.step + + def observe(state, context): + contexts.append(context) + return step(state, context) - with pytest.raises(ValidationError, match="runtime.control"): - runtime.run(steps=1) + controller.step = observe + runtime = HardwareRuntime(FakeRobotInterface(manifest.robot), realtime=False) + session = bind(runtime, controller, period_s=0.04) + assert runtime.run().success + assert session.period_s == 0.04 + assert [context.dt_s for context in contexts] == [0.04, 0.04, 0.04] + assert [context.elapsed_time_s for context in contexts] == pytest.approx([0, 0.04, 0.08]) -def test_hardware_factory_prepares_robot_for_explicit_control_binding(monkeypatch, manifest_factory): +@pytest.mark.parametrize("period,timeout", [(0.0, 0.1), (0.02, 0.0)]) +def test_session_validates_public_timing_boundary(manifest_factory, period, timeout): + manifest = manifest_factory() + with pytest.raises(ValidationError): + ControlSession( + controller=make_controller(manifest), + robot=FakeRobotInterface(manifest.robot), + command_binding=ConstantPlanarVelocityBinding((0, 0, 0)), + period_s=period, + state_timeout_s=timeout, + ) + + +def test_factory_produces_host_for_explicit_session_binding(monkeypatch, manifest_factory): manifest = manifest_factory() robot = FakeRobotInterface(manifest.robot) calls = [] @@ -69,24 +107,19 @@ def factory(config, context): 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 + runtime = create_hardware_runtime("fake", {}, context) + assert calls == [({}, context)] + assert runtime.robot is robot and not runtime.opened + controller = make_controller(manifest, steps=2) + session = bind(runtime, controller) + assert runtime.session is session and session.controller is controller assert robot.events == [] - with runtime: - assert runtime.opened - result = runtime.run(steps=1) - assert result.success + assert runtime.opened and runtime.session is session + result = runtime.run() + assert result.success and result.completed_steps == 2 + assert len(robot.commands) == 2 + assert runtime.session is session assert robot.events[-2:] == ["stop", "close"] assert not runtime.opened diff --git a/motrix_deploy/tests/test_simulation_config.py b/motrix_deploy/tests/test_simulation_config.py new file mode 100644 index 00000000..de7d6da4 --- /dev/null +++ b/motrix_deploy/tests/test_simulation_config.py @@ -0,0 +1,70 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Simulation scene, timing and sensor recipe decoding contracts.""" + +from copy import deepcopy + +import pytest + +from motrix_deploy.errors import ValidationError +from motrix_deploy.runtime.config import SensorBindings, SimulationRuntimeConfig +from motrix_env_core.config.scene import SceneCfg +from motrix_env_core.config.sim import SimCfg + + +def test_from_mapping_decodes_recipe_without_mutating_input(): + scene = SceneCfg() + nested = { + "physics": {"dt": 0.005, "solver_iterations": 25, "solver_tolerance": 0.0001, "gravity": (0.0, 0.0, -9.81)}, + "sensor_bindings": { + "base_angular_velocity": "gyro", + "base_linear_acceleration": "accelerometer", + "base_linear_velocity": "velocimeter", + }, + } + original = deepcopy(nested) + values = {"name": "generic-backend", "scene": scene, **nested} + config = SimulationRuntimeConfig.from_mapping(values, render=True, realtime=False) + assert config.scene is scene + assert isinstance(config.physics, SimCfg) + for name, value in nested["physics"].items(): + assert getattr(config.physics, name) == value + assert config.sensor_bindings == SensorBindings(**nested["sensor_bindings"]) + assert config.render is True + assert config.realtime is False + assert {name: values[name] for name in nested} == original + + +def test_from_mapping_supplies_independent_optional_defaults(): + values = {"scene": SceneCfg(), "physics": {"dt": 0.005, "solver_iterations": 25}} + first = SimulationRuntimeConfig.from_mapping(values) + second = SimulationRuntimeConfig.from_mapping(values) + assert first.sensor_bindings == SensorBindings() + assert first.render is False + assert first.realtime is None + assert first.sensor_bindings is not second.sensor_bindings + assert first.physics is not second.physics + assert set(values) == {"scene", "physics"} + + +def test_from_mapping_rejects_unknown_top_level_fields(): + unknown_name = "unrecognized_option" + values = {"scene": SceneCfg(), "physics": {"dt": 0.005, "solver_iterations": 25}, unknown_name: True} + with pytest.raises(ValidationError) as error: + SimulationRuntimeConfig.from_mapping(values) + assert error.value.path == "runtime" + assert unknown_name in str(error.value) + + +@pytest.mark.parametrize("section", ["physics", "sensor_bindings"]) +def test_from_mapping_rejects_unknown_nested_fields(section): + unknown_name = "unrecognized_option" + values = { + "scene": SceneCfg(), + "physics": {"dt": 0.005, "solver_iterations": 25}, + "sensor_bindings": {"base_angular_velocity": "gyro"}, + } + values[section][unknown_name] = True + with pytest.raises(TypeError, match=unknown_name): + SimulationRuntimeConfig.from_mapping(values) diff --git a/motrix_deploy/tests/test_simulation_runtime.py b/motrix_deploy/tests/test_simulation_runtime.py index 013a2460..7ea23d79 100644 --- a/motrix_deploy/tests/test_simulation_runtime.py +++ b/motrix_deploy/tests/test_simulation_runtime.py @@ -30,7 +30,13 @@ def __init__(self): def robot(self): return self._robot - def run(self, control, *, steps=None): + def advance_control_period(self): + pass + + def _physics_time_s(self): + return 0.0 + + def run(self): raise NotImplementedError calls = [] diff --git a/motrix_deploy/tests/test_task_plugins.py b/motrix_deploy/tests/test_task_plugins.py index 42cffe63..bb2e5875 100644 --- a/motrix_deploy/tests/test_task_plugins.py +++ b/motrix_deploy/tests/test_task_plugins.py @@ -1,15 +1,21 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Deployment task plugin discovery and loading boundary.""" +"""Deployment task plugin discovery and complete controller loading boundary.""" from types import SimpleNamespace from unittest.mock import Mock +import numpy as np import pytest +from controller_helpers import ConstantPolicy +from fake_robot import FakeRobotInterface from task_specs import TestDeployTask as FixtureDeployTask from motrix_deploy import task as task_module +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy.runtime.control import ControlSession +from motrix_deploy.runtime.hardware import HardwareRuntime from motrix_deploy.task import TASK_ENTRY_POINT_GROUP, available_tasks, create_task @@ -21,6 +27,10 @@ def entry_points(*, group): monkeypatch.setattr(task_module.metadata, "entry_points", entry_points) +def policy(): + return ConstantPolicy(4, np.zeros(2, np.float32)) + + def test_listing_is_sorted_and_lazy(monkeypatch): entries = [SimpleNamespace(name=name, load=Mock()) for name in ("other/v1", "test/v1")] _install(monkeypatch, entries) @@ -29,24 +39,36 @@ def test_listing_is_sorted_and_lazy(monkeypatch): entry.load.assert_not_called() -def test_selected_class_receives_artifact_contract(monkeypatch, manifest_factory): +def test_selected_class_receives_policy_and_budget_and_executes_as_session_controller(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) + model = policy() + task = create_task(manifest.task, manifest.robot, model, steps=2) assert isinstance(task, FixtureDeployTask) assert task.spec is manifest.task assert task.robot is manifest.robot + assert task.policy is model + assert task.steps == 2 selected.load.assert_called_once_with() other.load.assert_not_called() + backend = FakeRobotInterface(manifest.robot) + session = ControlSession(robot=backend, controller=task, period_s=0.02, state_timeout_s=0.1) + runtime = HardwareRuntime(backend, realtime=False) + runtime.bind_session(session) + result = runtime.run() + assert result.success + assert session.controller is task + assert len(task.states) == 2 + assert backend.events[-2:] == ["stop", "close"] 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) + create_task(manifest.task, manifest.robot, policy()) def test_duplicate_plugins_fail_before_loading(monkeypatch, manifest_factory): @@ -54,7 +76,7 @@ def test_duplicate_plugins_fail_before_loading(monkeypatch, 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) + create_task(manifest.task, manifest.robot, policy()) for entry in entries: entry.load.assert_not_called() @@ -64,4 +86,22 @@ 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) + create_task(manifest.task, manifest.robot, policy()) + + +def test_plugin_task_exposes_root_reset_step_and_stop(manifest_factory): + manifest = manifest_factory() + task = create_task(manifest.task, manifest.robot, policy()) + backend = FakeRobotInterface(manifest.robot) + backend.open() + try: + state = backend.read_state(0.1) + task.reset(state) + assert task.step(state, ControlContext(0, 0.0, None, dt_s=0.02)).complete + task.request_stop() + assert task.stop_requested + task.reset(state) + assert task.states == [state] + assert not task.stop_requested + finally: + backend.close() diff --git a/motrix_deploy_motrixsim/README.md b/motrix_deploy_motrixsim/README.md index 1812ccd9..2e8bb8ca 100644 --- a/motrix_deploy_motrixsim/README.md +++ b/motrix_deploy_motrixsim/README.md @@ -32,40 +32,38 @@ MotrixSim is supplied by `motrix-env-motrixsim`, using the workspace's pinned SD ## 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: +`create_simulation_runtime("motrixsim", config)` accepts a complete `SimulationRuntimeConfig` +and returns a prepared host with `runtime.robot` and its read-only `spec` available before +opening. Task, model, and command binding are application-owned components assembled +explicitly: ```python from motrix_deploy.runtime.control import ControlSession from motrix_deploy.runtime.factory import create_simulation_runtime +from motrix_deploy.task import create_task runtime = create_simulation_runtime("motrixsim", config) robot = runtime.robot -control = ControlSession( +task = create_task(task_spec, robot.spec, model, steps=50) +session = ControlSession( robot=robot, - task=task, - policy=policy, + controller=task, command_binding=command_binding, period_s=0.02, + state_timeout_s=0.1, ) -runtime.bind_control_session(control) +runtime.bind_session(session) with runtime: - result = runtime.run(steps=50) + result = runtime.run() ``` -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. +`config` contains the complete scene, physics, state sources, and rendering settings. The +control period must be an integer multiple of `config.physics.dt` and is validated when +the session is bound. The runtime owns the compiled MSD world, model, data, optional +viewer, and physics advancement; robot ports delegate I/O only. See the +[deployment tutorial](../docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md) +for the framework contracts (ControlSession, DeployTask, controller lifecycle, rollout +result semantics). ## Model, placement, and state contract @@ -94,13 +92,16 @@ sensor_bindings: 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. +Missing or incompatible sources fail rather than fabricating state. Public `RobotState` +does not expose world base position. Optional linear velocity comes only from its bound +sensor and remains `None` when unbound; 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 +ControlSession calls `Controller` with validated state and context and writes its command; +runtime advances one control interval using one scheduler. +Completion consumes no extra interval; task-specific ready and normal exit actions +are decisions of the concrete task, not runtime phases. Every physics substep recomputes servo torque from current joint state: ```text diff --git a/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/robot.py b/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/robot.py index 0a6c72cb..e903a1ce 100644 --- a/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/robot.py +++ b/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/robot.py @@ -47,6 +47,7 @@ def __init__(self, runtime: MotrixSimRuntime, spec: RobotSpec, actuator_names: t self._last_communication_ns = 0 self._health_reason = "simulation is not open" self._read: mtx.query.QueryProgram | None = None + self._read_fields: tuple[str, ...] = () 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) @@ -62,7 +63,6 @@ def __init__(self, runtime: MotrixSimRuntime, spec: RobotSpec, actuator_names: t "base_orientation_xyzw", "base_angular_velocity", "base_linear_acceleration", - "base_position", "base_linear_velocity", } ), @@ -102,6 +102,8 @@ def _validate_sensors(self) -> None: ): path = f"backend.sensor_bindings.{role}" local_name = getattr(self._runtime.config.sensor_bindings, role) + if local_name is None and role == "base_linear_velocity": + continue if local_name is None: raise ValidationError(path, "an explicit model-local sensor name", local_name) name = self._robot.resolve_name(local_name) @@ -128,11 +130,9 @@ def bind(self) -> None: 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.stop() self._health_reason = "" self._last_communication_ns = time.monotonic_ns() @@ -146,11 +146,11 @@ def refresh_reads(self) -> None: 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._read_fields = tuple(queries) self._write = model.compile_write({"torque": mtx.write.ActuatorCtrls(self._actuator_names)}).allocate(data) def open(self) -> None: @@ -167,16 +167,7 @@ def read_state(self, timeout_s: float) -> RobotState: # 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", - ) + name: np.array(self._read[name].reshape(-1), dtype=np.float32, copy=True) for name in self._read_fields } return RobotState( sample_time_ns=round(self._runtime.simulation_time_s * 1e9), @@ -250,6 +241,7 @@ def health(self) -> HealthStatus: ) def stop(self) -> None: + """Clear the command and apply zero torque immediately.""" self._command = None if self._opened and not self._closed and self._write is not None: self._write["torque"][:] = 0 diff --git a/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/runtime.py b/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/runtime.py index b9864ac3..fc8f28e1 100644 --- a/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/runtime.py +++ b/motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/runtime.py @@ -4,7 +4,6 @@ """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 @@ -21,8 +20,7 @@ from motrix_env_motrixsim.compiler import MotrixSimSceneCompiler if TYPE_CHECKING: - from motrix_deploy.runtime.control import ControlSession - from motrix_deploy.runtime.result import RolloutResult + from motrix_deploy.runtime.result import SimulationRolloutResult class MotrixSimRuntime(SimulationRuntime): @@ -94,7 +92,7 @@ def _derive_spec(model: mtx.SceneModel, robot: RobotCfg) -> tuple[RobotSpec, tup 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 + f"backend.joints.{name}.range", "session limits inside physical joint range", limits ) actuator_names.append(actuator.name) lower.append(limits[0]) @@ -127,16 +125,6 @@ def _prepare_motors(self, source_model: mtx.SceneModel, spec: RobotSpec, actuato 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) @@ -144,11 +132,10 @@ def _prepare_motors(self, source_model: mtx.SceneModel, spec: RobotSpec, actuato 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 + def _configure_session_period(self, period_s: float) -> None: + ratio = 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) + raise ValidationError("session.period_s", "an integer multiple of physics.dt", period_s) self.physics_substeps = round(ratio) def open(self) -> None: @@ -158,10 +145,10 @@ def open(self) -> None: self.model = self.world.build() try: self._robot.bind() + self._opened = True if self.viewer is not None: self.viewer.open(self.model) self.sync_viewer() - self._opened = True except BaseException: self.close() raise @@ -193,49 +180,25 @@ 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") + raise RuntimeError("Bind a session before advancing a session 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": + def _physics_time_s(self) -> float: + return self.simulation_time_s + + def run(self, *, realtime: bool | None = None) -> "SimulationRolloutResult": 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) + if self.session is None: + raise ValidationError("runtime.session", "a bound control session", None) 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, - ) + scheduler = (RealtimeScheduler if paced else FixedStepScheduler)(self.session.period_s) + return self._run_session(scheduler) def check_viewer_running(self) -> None: if self.viewer is not None and not self.viewer.is_running(): @@ -253,8 +216,6 @@ def get_keyboard_device(self): 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: diff --git a/motrix_deploy_motrixsim/tests/test_motion_state_and_native_servos.py b/motrix_deploy_motrixsim/tests/test_motion_state_and_native_servos.py new file mode 100644 index 00000000..fff11a61 --- /dev/null +++ b/motrix_deploy_motrixsim/tests/test_motion_state_and_native_servos.py @@ -0,0 +1,165 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Native SDK motion-state and software-PD behavior contracts.""" + +from dataclasses import replace +from types import SimpleNamespace + +import motrixsim as mtx +import numpy as np +from motrix_deploy_motrixsim.runtime import MotrixSimRuntime +from test_native_motrixsim_runtime import config + +from motrix_deploy.contracts import JointControlMode, JointServoCommand, JointTorqueCommand + + +def set_simulation_state( + runtime, + *, + root_position, + root_orientation_xyzw, + root_linear_velocity, + root_angular_velocity, + joint_position, + joint_velocity, +): + """Arrange native SDK state for sensor and control behavior tests only.""" + base = (runtime.scene.objs.robot.resolved_base_link_name,) + joints = tuple(runtime.scene.objs.robot.resolve_name(name) for name in runtime.robot.spec.joint_names) + program = runtime.model.compile_write( + { + "root_position": mtx.write.BodyPosition(base), + "root_orientation_xyzw": mtx.write.BodyRotation(base), + "root_linear_velocity": mtx.write.BodyLinearVelocity(base), + "root_angular_velocity": mtx.write.BodyAngularVelocity(base), + "joint_position": mtx.write.BodyJointPosition(joints), + "joint_velocity": mtx.write.BodyJointVelocity(joints), + }, + reset=True, + forward_kinematic=True, + ).allocate(runtime.data) + program["root_position"][:] = root_position + program["root_orientation_xyzw"][:] = root_orientation_xyzw + program["root_linear_velocity"][:] = root_linear_velocity + program["root_angular_velocity"][:] = root_angular_velocity + program["joint_position"][:] = joint_position + program["joint_velocity"][:] = joint_velocity + program.execute(runtime.data) + + +def motion(spec): + return SimpleNamespace( + root_position=np.asarray([1.2, -0.7, 2.0], dtype=np.float32), + root_orientation_xyzw=np.asarray([0.0, 0.0, np.sin(0.4), np.cos(0.4)], dtype=np.float32), + root_linear_velocity=np.asarray([0.4, -0.2, 0.3], dtype=np.float32), + root_angular_velocity=np.asarray([0.1, 0.3, -0.2], dtype=np.float32), + joint_position=spec.default_joint_position + np.float32(0.02), + joint_velocity=np.linspace(-0.2, 0.2, spec.joint_count, dtype=np.float32), + ) + + +def test_world_motion_sensor_frames_and_joint_mapping_with_instance_names(): + cfg = config() + cfg.scene.objs.robot.prefix = "motion_" + cfg.scene.objs.robot.suffix = "_instance" + with MotrixSimRuntime(cfg) as runtime: + initial = motion(runtime.robot.spec) + set_simulation_state(runtime, **vars(initial)) + + def check_motion(): + state = runtime.robot.read_state(0.1) + base = (runtime.scene.objs.robot.resolved_base_link_name,) + position = runtime.model.compile_query({"position": mtx.query.LinkPosition(base)}).allocate(runtime.data) + position.execute(runtime.data) + np.testing.assert_allclose(position["position"].reshape(3), initial.root_position, atol=1e-6) + np.testing.assert_allclose(state.base_orientation_xyzw, initial.root_orientation_xyzw, atol=1e-6) + np.testing.assert_allclose(state.joint_position, initial.joint_position, atol=1e-6) + np.testing.assert_allclose(state.joint_velocity, initial.joint_velocity, atol=1e-6) + angle = 0.8 + rotation = np.asarray( + [ + [np.cos(angle), -np.sin(angle), 0], + [np.sin(angle), np.cos(angle), 0], + [0, 0, 1], + ] + ) + np.testing.assert_allclose( + state.base_angular_velocity, rotation.T @ initial.root_angular_velocity, atol=1e-6 + ) + sensor_name = runtime.scene.objs.robot.resolve_name(runtime.config.sensor_bindings.base_linear_velocity) + sensor = next(sensor for sensor in runtime.world.sensors.frame if sensor.name == sensor_name) + site_read = runtime.model.compile_query( + { + "position": mtx.query.SitePosition((sensor.object_type.value,)), + } + ).allocate(runtime.data) + site_read.execute(runtime.data) + site_offset = site_read["position"].reshape(3) - initial.root_position + np.testing.assert_allclose( + state.base_linear_velocity, + initial.root_linear_velocity + np.cross(initial.root_angular_velocity, site_offset), + atol=1e-6, + ) + base = (runtime.scene.objs.robot.resolved_base_link_name,) + read = runtime.model.compile_query( + { + "linear": mtx.query.LinkLinearVelocity(base), + "angular": mtx.query.LinkAngularVelocity(base), + } + ).allocate(runtime.data) + read.execute(runtime.data) + np.testing.assert_allclose(read["linear"].reshape(3), initial.root_linear_velocity, atol=1e-6) + np.testing.assert_allclose(read["angular"].reshape(3), initial.root_angular_velocity, atol=1e-6) + return state + + check_motion() + + +def test_linear_velocity_capability_is_independent_of_sensor_binding(): + cfg = config() + capabilities = [] + for binding in (cfg.sensor_bindings.base_linear_velocity, None): + bound_config = replace(cfg, sensor_bindings=replace(cfg.sensor_bindings, base_linear_velocity=binding)) + with MotrixSimRuntime(bound_config) as runtime: + capabilities.append(runtime.robot.capabilities) + assert "base_linear_velocity" in runtime.robot.capabilities.state_fields + initial = motion(runtime.robot.spec) + initial.root_angular_velocity.fill(0) + set_simulation_state(runtime, **vars(initial)) + state = runtime.robot.read_state(0.1) + if binding is None: + assert state.base_linear_velocity is None + else: + np.testing.assert_allclose(state.base_linear_velocity, initial.root_linear_velocity, atol=1e-6) + assert capabilities[0] == capabilities[1] + + +def test_software_pd_uses_command_gains_velocity_and_feedforward(): + with MotrixSimRuntime(config()) as runtime: + assert all(isinstance(actuator, mtx.MotorActuator) for actuator in runtime.model.actuators) + assert runtime.robot.capabilities.control_modes == (JointControlMode.SERVO, JointControlMode.TORQUE) + assert runtime.robot.capabilities.stop_semantics == "zero_torque" + initial = motion(runtime.robot.spec) + set_simulation_state(runtime, **vars(initial)) + count = runtime.robot.spec.joint_count + command = JointServoCommand( + initial.joint_position + np.float32(0.01), + np.full(count, 0.15, dtype=np.float32), + np.full(count, 0.5, dtype=np.float32), + np.full(count, 12, dtype=np.float32), + np.full(count, 2, dtype=np.float32), + ) + runtime.robot.write_command(command) + runtime.robot.apply_control() + expected = ( + command.kp * (command.joint_position - initial.joint_position) + + command.kd * (command.joint_velocity - initial.joint_velocity) + + command.feedforward_torque + ) + np.testing.assert_allclose(runtime.model.get_actuator_ctrls(runtime.data).reshape(-1), expected, atol=1e-6) + runtime.robot.write_command(JointTorqueCommand(np.ones(count, dtype=np.float32))) + runtime.robot.apply_control() + np.testing.assert_array_equal(runtime.model.get_actuator_ctrls(runtime.data), 1) + runtime.reset() + np.testing.assert_array_equal(runtime.model.get_actuator_ctrls(runtime.data), 0) diff --git a/motrix_deploy_motrixsim/tests/test_native_motrixsim_runtime.py b/motrix_deploy_motrixsim/tests/test_native_motrixsim_runtime.py index 0929051b..68688db7 100644 --- a/motrix_deploy_motrixsim/tests/test_native_motrixsim_runtime.py +++ b/motrix_deploy_motrixsim/tests/test_native_motrixsim_runtime.py @@ -15,11 +15,11 @@ from motrix_deploy.contracts import JointControlMode, JointServoCommand, JointTorqueCommand from motrix_deploy.errors import ValidationError -from motrix_deploy.policy import NoOpPolicyRuntime +from motrix_deploy.policy import NoOpPolicy +from motrix_deploy.policy.processing import PolicyProcessor from motrix_deploy.runtime.config import SensorBindings, SimulationRuntimeConfig -from motrix_deploy.runtime.control import ControlSession +from motrix_deploy.runtime.control import ControlSession, PolicyController 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 @@ -46,13 +46,16 @@ def config(**kwargs): ) -class HoldTask(DeployTask): +class HoldPolicyProcessor(PolicyProcessor): def __init__(self, spec): self.spec = spec def reset(self, state, context): pass + def check_termination(self, state): + return None + def validate_command(self, command): pass @@ -64,15 +67,19 @@ def process_action(self, action): return JointServoCommand(self.spec.default_joint_position, z, z, z + 30, z + 1) -def session(runtime, period=0.006): - return ControlSession( +def bind_session(runtime, period=0.006, steps=2): + controller = PolicyController( + HoldPolicyProcessor(runtime.robot.spec), NoOpPolicy(runtime.robot.spec.joint_count), steps=steps + ) + session = ControlSession( + controller=controller, 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, ) + runtime.bind_session(session) + return controller @pytest.mark.parametrize("gravity,tolerance", [((0.3, -0.2, -4.1), 2e-7), ((-0.1, 0.4, -6.3), 3e-6)]) @@ -112,8 +119,7 @@ def test_native_factory_session_cadence_and_owner(monkeypatch): 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) + bind_session(runtime, steps=5) calls = [] original = runtime.robot.apply_control @@ -123,10 +129,10 @@ def count_apply(): monkeypatch.setattr(runtime.robot, "apply_control", count_apply) with runtime: - result = runtime.run(steps=5) + result = runtime.run() assert result.success assert result.completed_steps == 5 - assert result.simulation_time_s == pytest.approx(5 * control.period_s) + assert result.policy_simulation_time_s == pytest.approx(5 * 0.006) assert len(calls) == 5 * 3 assert runtime.opened runtime.robot.close() @@ -171,6 +177,49 @@ def test_native_servo_feedforward_snapshots_and_clipping(): np.testing.assert_array_equal(runtime.model.get_actuator_ctrls(runtime.data), 0) +@pytest.mark.parametrize("backend", ["motrixsim", "mujoco"]) +def test_stop_applies_zero_torque_across_substeps(backend): + if backend == "mujoco": + pytest.importorskip("mujoco") + runtime = create_simulation_runtime(backend, config()) + with runtime: + held = runtime.robot.read_state(0.1).joint_position.copy() + count = runtime.robot.spec.joint_count + runtime.robot.write_command(JointTorqueCommand(np.ones(count, dtype=np.float32))) + runtime.robot.apply_control() + runtime.robot.stop() + displaced = held + 0.02 + if backend == "motrixsim": + positions = np.asarray(runtime.data.dof_pos).reshape(-1).copy() + positions[runtime.robot.joint_dof_pos_indices] = displaced + runtime.data.set_dof_pos(positions, runtime.model) + runtime.model.forward_kinematic(runtime.data) + else: + runtime.data.qpos[runtime.robot._joint_qpos_indices] = displaced + runtime.mj.mj_forward(runtime.model, runtime.data) + for _ in range(3): + runtime.robot.read_state(0.1) + runtime.robot.apply_control() + controls = ( + np.asarray(runtime.model.get_actuator_ctrls(runtime.data)).reshape(-1) + if backend == "motrixsim" + else runtime.data.ctrl[runtime.robot._actuator_indices] + ) + np.testing.assert_array_equal(controls, 0.0) + if backend == "motrixsim": + runtime._step() + else: + runtime.mj.mj_step(runtime.model, runtime.data) + runtime.robot.stop() + runtime.robot.apply_control() + controls = ( + np.asarray(runtime.model.get_actuator_ctrls(runtime.data)).reshape(-1) + if backend == "motrixsim" + else runtime.data.ctrl[runtime.robot._actuator_indices] + ) + np.testing.assert_array_equal(controls, 0.0) + + def test_native_servo_substeps_only_query_joint_state(monkeypatch): runtime = MotrixSimRuntime(config()) with runtime: @@ -190,9 +239,13 @@ 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 + base = (runtime.scene.objs.robot.resolved_base_link_name,) + position = runtime.model.compile_query({"position": mtx.query.LinkPosition(base)}).allocate(runtime.data) + position.execute(runtime.data) + initial_position = position["position"].reshape(3).copy() + assert initial_position[0] == pytest.approx(0.2) + assert initial_position[1] == pytest.approx(-0.1) + assert initial_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): @@ -201,8 +254,9 @@ def test_native_repeated_reset_preserves_compiled_placement_and_sensor_state(): runtime.reset() reset = runtime.robot.read_state(0.1) assert reset.sample_time_ns == 0 + position.execute(runtime.data) + np.testing.assert_allclose(position["position"].reshape(3), initial_position, atol=1e-6) for field in ( - "base_position", "base_orientation_xyzw", "joint_position", "joint_velocity", @@ -213,7 +267,9 @@ def test_native_repeated_reset_preserves_compiled_placement_and_sensor_state(): 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) + position = runtime.model.compile_query({"position": mtx.query.LinkPosition(base)}).allocate(runtime.data) + position.execute(runtime.data) + np.testing.assert_allclose(position["position"].reshape(3), initial_position, atol=1e-6) def test_native_sensor_bindings_are_actual_live_native_values(): @@ -245,8 +301,8 @@ def test_native_instance_names_preserve_artifact_local_robot_contract(): 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 + bind_session(runtime) + assert runtime.run().success def test_native_motor_source_supported(): @@ -273,7 +329,7 @@ def test_native_motor_source_supported(): 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)) + bind_session(runtime, period) runtime.close() @@ -294,7 +350,7 @@ def test_native_explicit_sensor_semantics(role, name): pass -@pytest.mark.parametrize("role", ["base_angular_velocity", "base_linear_acceleration", "base_linear_velocity"]) +@pytest.mark.parametrize("role", ["base_angular_velocity", "base_linear_acceleration"]) def test_native_missing_sensor_fails_at_model_binding(role): cfg = config() cfg = replace(cfg, sensor_bindings=replace(cfg.sensor_bindings, **{role: None})) @@ -339,9 +395,9 @@ def test_native_artifact_mapping_same_contract(): 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)) + bind_session(runtime) with runtime: - assert runtime.run(steps=2).success + assert runtime.run().success @pytest.mark.parametrize("physics", [{}, {"dt": 0.004}, {"solver_iterations": None}]) @@ -389,8 +445,7 @@ def close(self): 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) + bind_session(runtime, steps=5) with runtime: assert runtime.viewer.is_running() assert device is runtime.get_keyboard_device() diff --git a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/robot.py b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/robot.py index 99b283b0..70d2ca8d 100644 --- a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/robot.py +++ b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/robot.py @@ -92,7 +92,6 @@ def bind(self) -> None: "base_orientation_xyzw", "base_angular_velocity", "base_linear_acceleration", - "base_position", "base_linear_velocity", } ), @@ -129,8 +128,10 @@ def write_command(self, command: RobotCommand) -> None: 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 + # Independently imported model/task limits can round differently at float32 boundaries. + position_tolerance = 1e-6 + if np.any(command.joint_position < self._spec.position_lower - position_tolerance) or np.any( + command.joint_position > self._spec.position_upper + position_tolerance ): raise ValidationError( "command.joint_position", "inside RobotSpec position range", command.joint_position @@ -147,6 +148,13 @@ def write_command(self, command: RobotCommand) -> None: # 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}) + if isinstance(self._command, JointServoCommand): + np.clip( + self._command.joint_position, + self._spec.position_lower, + self._spec.position_upper, + out=self._command.joint_position, + ) self._last_communication_ns = time.monotonic_ns() def apply_control(self) -> None: @@ -177,11 +185,11 @@ def health(self) -> HealthStatus: ) def stop(self) -> None: + """Clear the cached command and zero actuator torque.""" 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 + 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.""" @@ -333,6 +341,8 @@ def _bind_sensors(self) -> None: path = f"runtime.sensor_bindings.{role}" local_name = getattr(self.config.sensor_bindings, role) if local_name is None: + if role == "base_linear_velocity": + continue 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}") @@ -379,8 +389,9 @@ def _state(self) -> RobotState: 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"), + base_linear_velocity=( + self._sensor("base_linear_velocity") if "base_linear_velocity" in self._sensor_slices else None + ), ) def _sensor(self, role: str) -> NDArray[np.float32]: diff --git a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/runtime.py b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/runtime.py index 9cd8e238..2d5ec553 100644 --- a/motrix_deploy_mujoco/src/motrix_deploy_mujoco/runtime.py +++ b/motrix_deploy_mujoco/src/motrix_deploy_mujoco/runtime.py @@ -9,9 +9,8 @@ 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 typing import TYPE_CHECKING, cast from motrix_deploy.contracts import RobotSpec from motrix_deploy.robot.interface import RobotInterface @@ -22,13 +21,12 @@ 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.result import SimulationRolloutResult from motrix_deploy.runtime.scheduler import FixedStepScheduler, RealtimeScheduler from motrix_deploy_mujoco.robot import Robot from motrix_deploy_mujoco.viewer import MujocoGlfwViewer @@ -36,8 +34,6 @@ from motrix_env_core.config.scene.base import RobotCfg from motrix_env_mujoco.compiler import MuJoCoSceneCompiler -CommandT = TypeVar("CommandT") - logger = logging.getLogger(__name__) @@ -67,7 +63,7 @@ def __init__( 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._configure_session_period(control_period_s) self.realtime = config.realtime self._closed = False self._source_spec: mujoco.MjSpec | None = None @@ -78,20 +74,13 @@ def __init__( def robot(self) -> RobotInterface: return self._robot - def _configure_control_period(self, period_s: float) -> None: + def _configure_session_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) + raise ValidationError("session.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) @@ -172,72 +161,26 @@ def reset( 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. + def _physics_time_s(self) -> float: + return float(self.data.time) - 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. - """ + def run(self, *, realtime: bool | None = None) -> SimulationRolloutResult: 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, - ) + if self.session is None: + raise ValidationError("runtime.session", "a bound control session", None) + 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)(self.session.period_s) + return self._run_session(scheduler) 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") + raise RuntimeError("Bind a session before advancing a session period") self.check_viewer_running() for _ in range(self.physics_substeps): self._robot.apply_control() @@ -257,8 +200,6 @@ 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 diff --git a/motrix_deploy_mujoco/tests/control_helpers.py b/motrix_deploy_mujoco/tests/control_helpers.py index e9c64a81..0a962390 100644 --- a/motrix_deploy_mujoco/tests/control_helpers.py +++ b/motrix_deploy_mujoco/tests/control_helpers.py @@ -6,12 +6,12 @@ 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_deploy.policy.processing import PolicyProcessor +from motrix_deploy.runtime.context import ControlContext from motrix_env_core.input import PlanarVelocityCommand -class DummyServoTask(DeployTask[PlanarVelocityCommand]): +class DummyServoPolicyProcessor(PolicyProcessor[PlanarVelocityCommand]): """Keep generic driver tests independent of examples and trained artifacts.""" def __init__(self, robot: RobotSpec, kp: FloatArray, kd: FloatArray) -> None: @@ -20,10 +20,10 @@ def __init__(self, robot: RobotSpec, kp: FloatArray, kd: FloatArray) -> None: self.kd = kd self._zeros = np.zeros(robot.joint_count, dtype=np.float32) - def reset(self, state: RobotState, context: PolicyContext[PlanarVelocityCommand]) -> None: + def reset(self, state: RobotState, context: ControlContext[PlanarVelocityCommand]) -> None: pass - def build_observation(self, state: RobotState, context: PolicyContext[PlanarVelocityCommand]) -> FloatArray: + def build_observation(self, state: RobotState, context: ControlContext[PlanarVelocityCommand]) -> FloatArray: return self._zeros.copy() def process_action(self, action: FloatArray) -> JointServoCommand: @@ -35,6 +35,9 @@ def process_action(self, action: FloatArray) -> JointServoCommand: kd=self.kd, ) + def check_termination(self, state: RobotState) -> str | None: + return None + 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") + raise ValueError("DummyServoPolicyProcessor accepts one zero velocity command") diff --git a/motrix_deploy_mujoco/tests/test_examples.py b/motrix_deploy_mujoco/tests/test_examples.py index 139a9b5f..b16f31c9 100644 --- a/motrix_deploy_mujoco/tests/test_examples.py +++ b/motrix_deploy_mujoco/tests/test_examples.py @@ -13,7 +13,7 @@ import numpy as np import pytest -from control_helpers import DummyServoTask +from control_helpers import DummyServoPolicyProcessor ROOT = Path(__file__).parents[2] EXAMPLES = ROOT / "examples" @@ -22,13 +22,12 @@ @pytest.fixture def onnx_policy(tmp_path, control_scene_config, control_gains): - """Generate raw ONNX locally; never depend on a trained checkout policy.""" + """Generate raw ONNX locally; never depend on a trained checkout controller.""" 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 + from motrix_deploy_tasks.tasks.go2_walk import Go2WalkPolicyProcessor, Go2WalkTaskSpec runtime = MujocoRuntime(control_scene_config, control_period_s=0.02) robot = runtime.robot.spec @@ -42,22 +41,19 @@ def onnx_policy(tmp_path, control_scene_config, control_gains): 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 + task = Go2WalkPolicyProcessor(task_spec, robot) + from motrix_deploy.runtime.context import ControlContext 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(), + context = ControlContext( + step=0, elapsed_time_s=0.0, command=ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)).read_command(), dt_s=0.02 ) state = runtime.robot.read_state(0.1) task.reset(state, context) @@ -115,8 +111,8 @@ def test_programmatic_example_uses_shared_control_execution(tmp_path, onnx_polic 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. + assert result["policy_simulation_time_s"] == pytest.approx(0.06) + assert "latency" in result # Shared session 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"] @@ -125,10 +121,10 @@ def test_programmatic_example_uses_shared_control_execution(tmp_path, onnx_polic 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.policy import NoOpPolicy 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.control import ControlSession, PolicyController from motrix_deploy.runtime.factory import create_simulation_runtime from motrix_env_core.input import ConstantPlanarVelocityBinding @@ -142,27 +138,28 @@ def test_simulation_factory_supports_explicit_bound_control_and_world_ownership( assert isinstance(robot, RobotInterface) assert not runtime.opened assert runtime.model is None and runtime.data is None - control = ControlSession( + controller = PolicyController( + DummyServoPolicyProcessor(robot.spec, *control_gains), NoOpPolicy(robot.spec.joint_count), steps=2 + ) + session = ControlSession( + controller=controller, 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 + runtime.bind_session(session) + assert runtime.control_period_s == 0.012 assert runtime.physics_substeps == 6 - assert runtime.control is control and control.robot is robot + assert runtime.session is session assert not runtime.opened assert runtime.model is None and runtime.data is None with runtime: - result = runtime.run(steps=2) + result = runtime.run() 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.robot is robot and runtime.session is session + # ControlSession 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() @@ -172,23 +169,21 @@ def test_simulation_factory_supports_explicit_bound_control_and_world_ownership( 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.runtime.context import ControlContext 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) + task = DummyServoPolicyProcessor(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(), + context = ControlContext( + step=0, elapsed_time_s=0.0, command=ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)).read_command(), dt_s=0.02 ) observation = task.build_observation(before, context) np.testing.assert_array_equal(observation, np.zeros(spec.joint_count, dtype=np.float32)) @@ -202,7 +197,7 @@ def test_robot_commands_do_not_advance_or_own_the_world(control_scene_config, co assert advanced.sample_time_ns > before.sample_time_ns interface.stop() interface.close() - # Closing the robot port releases control, not the shared simulation. + # Closing the robot port releases session, 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() diff --git a/motrix_deploy_mujoco/tests/test_initial_state_and_native_servos.py b/motrix_deploy_mujoco/tests/test_initial_state_and_native_servos.py new file mode 100644 index 00000000..756a00d4 --- /dev/null +++ b/motrix_deploy_mujoco/tests/test_initial_state_and_native_servos.py @@ -0,0 +1,130 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Robot motion coordinates, sensor bindings and software PD contracts.""" + +from dataclasses import replace +from types import SimpleNamespace + +import numpy as np + +from motrix_deploy.contracts import JointControlMode, JointServoCommand +from motrix_deploy_mujoco.runtime import MujocoRuntime + + +def _motion_fixture(spec): + return SimpleNamespace( + root_position=np.array([1.2, -0.5, 2.0], dtype=np.float32), + root_orientation_xyzw=np.array([0, 0, np.sqrt(0.5), np.sqrt(0.5)], dtype=np.float32), + root_linear_velocity=np.array([0.2, -0.4, 0.6], dtype=np.float32), + root_angular_velocity=np.array([0.1, 0.3, -0.2], dtype=np.float32), + joint_position=spec.default_joint_position + np.linspace(-0.01, 0.01, spec.joint_count, dtype=np.float32), + joint_velocity=np.linspace(-0.3, 0.3, spec.joint_count, dtype=np.float32), + ) + + +def _set_motion_fixture(runtime, motion): + """Write native MuJoCo data only to exercise robot state decoding in tests.""" + model, data = runtime.model, runtime.data + cfg = runtime.scene.objs.robot + root_joint = model.body_jntadr[model.body(cfg.resolved_base_link_name).id] + qpos_address = model.jnt_qposadr[root_joint] + dof_address = model.jnt_dofadr[root_joint] + quaternion = motion.root_orientation_xyzw[[3, 0, 1, 2]].astype(np.float64) + rotation = np.empty(9) + runtime.mj.mju_quat2Mat(rotation, quaternion) + data.qpos[qpos_address : qpos_address + 3] = motion.root_position + data.qpos[qpos_address + 3 : qpos_address + 7] = quaternion + data.qvel[dof_address : dof_address + 3] = motion.root_linear_velocity + data.qvel[dof_address + 3 : dof_address + 6] = rotation.reshape(3, 3).T @ motion.root_angular_velocity + for index, name in enumerate(cfg.key_pose.joint_names): + joint = model.joint(cfg.resolve_name(name)) + data.qpos[joint.qposadr] = motion.joint_position[index] + data.qvel[joint.dofadr] = motion.joint_velocity[index] + runtime.mj.mj_forward(model, data) + + +def test_robot_state_maps_root_coordinates_gyro_world_velocity_and_canonical_joints(control_scene_config): + # Reverse canonical order and prefix model names: neither vector may use raw MuJoCo order. + cfg = control_scene_config.scene.objs.robot + cfg.prefix = "test_" + names = cfg.key_pose.joint_names + cfg.key_pose.joint_names = tuple(reversed(names)) + cfg.key_pose.poses = {name: tuple(reversed(pose)) for name, pose in cfg.key_pose.poses.items()} + with MujocoRuntime(control_scene_config, 0.02) as runtime: + motion = _motion_fixture(runtime.robot.spec) + _set_motion_fixture(runtime, motion) + state = runtime.robot.read_state(0.1) + np.testing.assert_allclose(runtime.data.xpos[runtime.robot._base_body_id], motion.root_position, atol=1e-6) + np.testing.assert_allclose(state.base_orientation_xyzw, motion.root_orientation_xyzw, atol=1e-6) + np.testing.assert_allclose(state.base_angular_velocity, [0.3, -0.1, -0.2], atol=1e-6) + # The world-frame sensor includes omega cross offset for its attached site. + sensor = runtime.model.sensor(cfg.resolve_name("global_linvel")) + site_position = runtime.data.site_xpos[sensor.objid[0]] + offset = site_position - runtime.data.xpos[runtime.robot._base_body_id] + expected_velocity = motion.root_linear_velocity + np.cross(motion.root_angular_velocity, offset) + np.testing.assert_allclose(state.base_linear_velocity, expected_velocity, atol=1e-6) + np.testing.assert_array_equal(state.joint_position, motion.joint_position) + np.testing.assert_array_equal(state.joint_velocity, motion.joint_velocity) + + +def test_linear_velocity_capability_is_independent_of_sensor_binding(control_scene_config): + capabilities = [] + for binding in (control_scene_config.sensor_bindings.base_linear_velocity, None): + config = replace( + control_scene_config, + sensor_bindings=replace(control_scene_config.sensor_bindings, base_linear_velocity=binding), + ) + with MujocoRuntime(config) as runtime: + capabilities.append(runtime.robot.capabilities) + assert "base_linear_velocity" in runtime.robot.capabilities.state_fields + motion = _motion_fixture(runtime.robot.spec) + motion.root_angular_velocity.fill(0) + _set_motion_fixture(runtime, motion) + state = runtime.robot.read_state(0.1) + if binding is None: + assert state.base_linear_velocity is None + else: + np.testing.assert_allclose(state.base_linear_velocity, motion.root_linear_velocity, atol=1e-6) + assert capabilities[0] == capabilities[1] + + +def test_software_pd_uses_all_command_fields_each_substep_and_zeroes_on_stop(control_scene_config, monkeypatch): + with MujocoRuntime(control_scene_config, 0.02) as runtime: + spec = runtime.robot.spec + robot = runtime.robot + indices = robot._actuator_indices + assert robot.capabilities.control_modes == (JointControlMode.SERVO, JointControlMode.TORQUE) + assert robot.capabilities.stop_semantics == "zero_torque" + assert np.all(runtime.model.actuator_biastype[indices] == runtime.mj.mjtBias.mjBIAS_NONE) + command = JointServoCommand( + joint_position=spec.default_joint_position.copy(), + joint_velocity=np.full(spec.joint_count, 0.2, dtype=np.float32), + feedforward_torque=np.full(spec.joint_count, 0.3, dtype=np.float32), + kp=np.full(spec.joint_count, 3.0, dtype=np.float32), + kd=np.full(spec.joint_count, 0.7, dtype=np.float32), + ) + robot.write_command(command) + snapshots = [] + step = runtime.mj.mj_step + + def record_step(model, data): + expected = np.clip( + command.kp * (command.joint_position - data.qpos[robot._joint_qpos_indices]) + + command.kd * (command.joint_velocity - data.qvel[robot._joint_qvel_indices]) + + command.feedforward_torque, + -spec.torque_limit, + spec.torque_limit, + ) + np.testing.assert_allclose(data.ctrl[indices], expected, atol=1e-7) + snapshots.append(data.ctrl[indices].copy()) + step(model, data) + + monkeypatch.setattr(runtime.mj, "mj_step", record_step) + runtime.advance_control_period() + assert len(snapshots) == runtime.physics_substeps + assert not np.array_equal(snapshots[0], snapshots[-1]) + robot.stop() + np.testing.assert_array_equal(runtime.data.ctrl[indices], 0) + robot.apply_control() + np.testing.assert_array_equal(runtime.data.ctrl[indices], 0) diff --git a/motrix_deploy_mujoco/tests/test_joint_control.py b/motrix_deploy_mujoco/tests/test_joint_control.py index a3c9d24c..d78881e4 100644 --- a/motrix_deploy_mujoco/tests/test_joint_control.py +++ b/motrix_deploy_mujoco/tests/test_joint_control.py @@ -87,6 +87,24 @@ def test_command_limits_are_validated_before_application(control_scene_config, k np.testing.assert_array_equal(runtime.data.ctrl, 0) +@pytest.mark.parametrize("bound,direction", [("position_lower", -1.0), ("position_upper", 1.0)]) +def test_servo_accepts_float32_boundary_rounding_but_rejects_real_limit_violation( + control_scene_config, bound, direction +): + # Task and backend limits may originate from independently rounded model imports. + with MujocoRuntime(control_scene_config, 0.02) as runtime: + spec = runtime.robot.spec + limit = getattr(spec, bound) + runtime.robot.write_command(_servo(spec, joint_position=limit.copy())) + np.testing.assert_array_equal(runtime.robot._command.joint_position, limit) + rounded_target = limit + np.float32(direction * 2e-7) + runtime.robot.write_command(_servo(spec, joint_position=rounded_target)) + np.testing.assert_array_equal(runtime.robot._command.joint_position, limit) + np.testing.assert_array_equal(rounded_target, limit + np.float32(direction * 2e-7)) + with pytest.raises(ValidationError, match="command.joint_position"): + runtime.robot.write_command(_servo(spec, joint_position=limit + np.float32(direction * 2e-5))) + + 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 diff --git a/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py b/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py index 7b97cacd..75213408 100644 --- a/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py +++ b/motrix_deploy_mujoco/tests/test_mujoco_deploy_backend.py @@ -208,7 +208,7 @@ def test_state_sensor_mapping_rejects_unknown_roles() -> None: SimulationRuntimeConfig.from_mapping(values) -@pytest.mark.parametrize("role", ["base_angular_velocity", "base_linear_acceleration", "base_linear_velocity"]) +@pytest.mark.parametrize("role", ["base_angular_velocity", "base_linear_acceleration"]) 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})) @@ -414,7 +414,7 @@ def unexpected_lookup(*args: Any) -> int: 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(backend.data.xpos[backend.robot._base_body_id], backend.model.qpos0[:3]) np.testing.assert_allclose(state.base_angular_velocity, [0.4, -0.5, 0.6], atol=1e-6) backend.close() @@ -470,7 +470,7 @@ def test_direct_scene_cfg_headless_uses_supplied_robot_floor_and_placement() -> 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(backend.data.xpos[backend.robot._base_body_id], 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) diff --git a/motrix_deploy_mujoco/tests/test_scene_session.py b/motrix_deploy_mujoco/tests/test_scene_session.py index a4df8a3a..02da1b2b 100644 --- a/motrix_deploy_mujoco/tests/test_scene_session.py +++ b/motrix_deploy_mujoco/tests/test_scene_session.py @@ -89,7 +89,7 @@ def test_file_scene_and_custom_objects_preserved_placement_has_no_double_transfo 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(backend.data.xpos[backend.robot._base_body_id], 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) @@ -131,7 +131,7 @@ def test_robot_attach_transform_and_init_key_pose_are_reset_once(rotation) -> No 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(runtime.data.xpos[runtime.robot._base_body_id], 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 @@ -151,7 +151,7 @@ def test_environment_scene_is_resolved_by_application_before_backend() -> None: 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]) + np.testing.assert_allclose(backend.data.xpos[backend.robot._base_body_id], backend.model.qpos0[:3]) finally: backend.close() diff --git a/motrix_deploy_mujoco/tests/test_simulation_driver.py b/motrix_deploy_mujoco/tests/test_simulation_driver.py index 545c785b..bb63d87c 100644 --- a/motrix_deploy_mujoco/tests/test_simulation_driver.py +++ b/motrix_deploy_mujoco/tests/test_simulation_driver.py @@ -1,39 +1,48 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Physics-owned control cadence, pacing, and shared lifecycle integration.""" +"""Physics-owned session cadence, pacing, and shared lifecycle integration.""" import numpy as np import pytest -from control_helpers import DummyServoTask +from control_helpers import DummyServoPolicyProcessor from motrix_deploy.errors import ValidationError -from motrix_deploy.policy import NoOpPolicyRuntime -from motrix_deploy.runtime.control import ControlSession +from motrix_deploy.policy import NoOpPolicy +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy.runtime.control import ControlSession, PolicyController +from motrix_deploy.runtime.lifecycle import ControllerStep +from motrix_deploy.runtime.result import LatencySummary, SimulationRolloutResult +from motrix_deploy.runtime.scheduler import FixedStepScheduler 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), +def bind_task(simulation, task, *, steps=3, period_s=None): + period_s = simulation.control_period_s if period_s is None else period_s + controller = PolicyController(task, NoOpPolicy(simulation.robot.spec.joint_count), steps=steps) + session = ControlSession( + controller=controller, + robot=simulation.robot, command_binding=ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)), - period_s=simulation.control_period_s, + period_s=period_s, state_timeout_s=0.1, ) + simulation.bind_session(session) + return controller + + +@pytest.fixture +def driver(control_scene_config, control_gains): + simulation = MujocoRuntime(control_scene_config, control_period_s=0.02) + task = DummyServoPolicyProcessor(simulation.robot.spec, *control_gains) + controller = bind_task(simulation, task) with simulation: - yield simulation, control, interface, task + yield simulation, controller, simulation.robot, task def test_physics_substeps_and_control_sample_times(driver, monkeypatch): - simulation, control, interface, task = driver + simulation, session, interface, task = driver events = [] samples = [] contexts = [] @@ -58,20 +67,47 @@ def record_observation(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) + result = simulation.run() 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 isinstance(result, SimulationRolloutResult) + assert result.policy_simulation_time_s == pytest.approx(0.06) + assert isinstance(result.latency["read"], LatencySummary) + report = result.to_dict() + assert report["policy_simulation_time_s"] == pytest.approx(0.06) + assert report["latency"]["read"]["mean_ms"] == result.latency["read"].mean_ms 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): +def test_simulation_duration_excludes_nonpolicy_command_intervals(driver, monkeypatch): + simulation, controller, _, _ = driver + step = controller.step + decisions = 0 + + def with_ready_interval(state, context): + nonlocal decisions + decisions += 1 + if decisions == 1: + # A deterministic ready command has physical duration but no model tick. + return ControllerStep( + command=controller.policy_io.process_action( + np.zeros(simulation.robot.spec.joint_count, dtype=np.float32) + ) + ) + return step(state, context) + + monkeypatch.setattr(controller, "step", with_ready_interval) + result = simulation.run() + assert result.success and result.completed_steps == 3 + assert simulation.data.time == pytest.approx(0.08) + assert result.policy_simulation_time_s == pytest.approx(0.06) + + +def test_torque_controller_preserves_control_and_physics_cadence(driver, monkeypatch): from motrix_deploy.contracts import JointTorqueCommand simulation, _, interface, task = driver @@ -92,33 +128,27 @@ def record_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) + result = simulation.run() 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) + assert result.policy_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_controller_step_never_writes_or_advances_physics(driver): + simulation, controller, robot, _ = driver + state = robot.read_state(0.1) + context = ControlContext(0, 0.0, ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)).read_command(), dt_s=0.02) + before_time = simulation.data.time + before_control = simulation.data.ctrl.copy() + controller.reset(state) + decision = controller.step(state, context) + assert decision.command is not None and not decision.complete + assert simulation.data.time == before_time + np.testing.assert_array_equal(simulation.data.ctrl, before_control) def test_headless_driver_is_deterministic_and_never_paces(monkeypatch, control_scene_config, control_gains): @@ -129,19 +159,11 @@ def test_headless_driver_is_deterministic_and_never_paces(monkeypatch, control_s traces = [] for _ in range(2): simulation = MujocoRuntime(config, control_period_s=0.02) - task = DummyServoTask(simulation.robot.spec, *control_gains) - interface = simulation.robot + task = DummyServoPolicyProcessor(simulation.robot.spec, *control_gains) 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 + bind_task(simulation, task, steps=4) + result = simulation.run() + assert result.success traces.append(result.trace_sha256) assert traces[0] == traces[1] @@ -149,13 +171,12 @@ def test_headless_driver_is_deterministic_and_never_paces(monkeypatch, control_s def test_realtime_paces_every_complete_interval_without_extra_tick(driver, monkeypatch): from motrix_deploy_mujoco import runtime - simulation, control, _, _ = driver + simulation, session, _, _ = driver waits = [] - class Scheduler: - overrun_count = 2 - + class Scheduler(FixedStepScheduler): def __init__(self, period_s): + super().__init__(period_s) assert period_s == simulation.control_period_s def reset(self): @@ -165,14 +186,13 @@ def wait(self, step): waits.append(step) monkeypatch.setattr(runtime, "RealtimeScheduler", Scheduler) - result = simulation.run(control, steps=3, realtime=True) + result = simulation.run(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 + simulation, session, interface, _ = driver step = simulation.mj.mj_step calls = 0 @@ -184,33 +204,33 @@ def fail_mid_interval(model, data): step(model, data) monkeypatch.setattr(simulation.mj, "mj_step", fail_mid_interval) - result = simulation.run(control, steps=3) + result = simulation.run() 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 result.policy_simulation_time_s == pytest.approx(2 * simulation.config.physics.dt) + assert result.real_time_factor == pytest.approx(result.policy_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 + assert not interface.health().healthy np.testing.assert_array_equal(simulation.data.ctrl, np.zeros(simulation.model.nu)) - control.stop() # Shared cleanup remains idempotent after driver failure. + interface.stop() # Backend cleanup remains idempotent after driver failure. -def test_start_exception_still_cleans_shared_control(driver, monkeypatch): - simulation, control, interface, _ = driver +def test_controller_reset_exception_still_cleans_robot(driver, monkeypatch): + simulation, session, interface, _ = driver - def failed_start(): + def failed_reset(state): raise RuntimeError("start failed") - monkeypatch.setattr(control, "start", failed_start) - result = simulation.run(control, steps=3) + monkeypatch.setattr(session, "reset", failed_reset) + result = simulation.run() assert result.exit_reason == "backend_error" and result.error == "start failed" - assert result.completed_steps == 0 and result.simulation_time_s == 0.0 + assert result.completed_steps == 0 and result.policy_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, session, _, task = driver simulation.advance_control_period() before = simulation.data.time contexts = [] @@ -221,84 +241,107 @@ def record_context(state, context): return observe(state, context) monkeypatch.setattr(task, "build_observation", record_context) - result = simulation.run(control, steps=2) + result = simulation.run() 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) + assert contexts == pytest.approx([0.0, simulation.control_period_s, 2 * simulation.control_period_s]) + assert result.policy_simulation_time_s == pytest.approx(3 * simulation.control_period_s) + assert simulation.data.time - before == pytest.approx(result.policy_simulation_time_s) def test_start_termination_does_not_advance_physics(driver, monkeypatch): - simulation, control, interface, task = driver + simulation, session, interface, task = driver monkeypatch.setattr(task, "check_termination", lambda state: "done") - result = simulation.run(control, steps=3) + result = simulation.run() assert not result.success and result.exit_reason == "task_terminated" - assert result.completed_steps == 0 and result.simulation_time_s == 0.0 + assert result.completed_steps == 0 and result.policy_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 + simulation, session, _, task = driver monkeypatch.setattr(task, "check_termination", lambda state: "done" if state.sample_time_ns > 0 else None) - result = simulation.run(control, steps=3) + result = simulation.run() assert result.exit_reason == "task_terminated" and result.completed_steps == 1 - assert result.simulation_time_s == pytest.approx(simulation.control_period_s) + assert result.policy_simulation_time_s == pytest.approx(simulation.control_period_s) def test_viewer_interrupt_uses_shared_cleanup(driver, monkeypatch): - simulation, control, interface, _ = driver + simulation, session, interface, _ = driver def interrupt(): raise KeyboardInterrupt("viewer closed") monkeypatch.setattr(simulation, "check_viewer_running", interrupt) - result = simulation.run(control, steps=3) + result = simulation.run() assert result.exit_reason == "interrupted" and result.completed_steps == 0 - assert result.simulation_time_s == 0.0 and not interface.health().healthy + assert result.policy_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_bound_controller_period_controls_physics_cadence(driver): + simulation, _, _, task = driver + # A fresh runtime can bind a different integral session period. + simulation.close() + with MujocoRuntime(simulation.config) as runtime: + bind_task(runtime, task, steps=2, period_s=0.01) + result = runtime.run() + assert result.success + assert runtime.control_period_s == 0.01 + assert result.policy_simulation_time_s == pytest.approx(0.02) -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), +def test_binding_rejects_nonintegral_period_without_changing_cadence(driver): + simulation, controller, _, _ = driver + period_s = simulation.control_period_s + substeps = simulation.physics_substeps + invalid_period = simulation.sim.dt * 1.5 + fresh = MujocoRuntime(simulation.config, control_period_s=period_s) + invalid = ControlSession( + controller=PolicyController(controller.policy_io, controller.policy, steps=3), + robot=fresh.robot, command_binding=ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)), - period_s=0.01, + period_s=invalid_period, 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 + # An unbound host rejects non-integral cadence before claiming the session. + bound = simulation.session + with pytest.raises(ValidationError, match="session.period_s"): + fresh.bind_session(invalid) + assert fresh.session is None + assert simulation.session is bound + fresh.close() 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 +def test_runtime_binding_rejects_foreign_robot_and_conflicting_control(driver): + simulation, controller, robot, _ = driver + bound = simulation.session + conflict = ControlSession( + controller=controller, + robot=robot, + command_binding=ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)), + period_s=bound.period_s, + state_timeout_s=0.1, + ) + with pytest.raises(ValidationError, match="bound"): + simulation.bind_session(conflict) + foreign_runtime = MujocoRuntime(simulation.config, control_period_s=bound.period_s) + try: + with pytest.raises(ValidationError, match="robot"): + foreign_runtime.bind_session(bound) + assert foreign_runtime.session is None + assert simulation.session is bound + finally: + foreign_runtime.close() + - 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) +def test_final_advanced_state_is_checked_before_controller_completion(driver, monkeypatch): + simulation, _, _, task = driver + monkeypatch.setattr( + task, "check_termination", lambda state: "last interval failed" if state.sample_time_ns >= 60_000_000 else None + ) + result = simulation.run() + assert not result.success + assert result.exit_reason == "task_terminated" + assert result.completed_steps == 3 + assert result.policy_simulation_time_s == pytest.approx(0.06) diff --git a/motrix_deploy_mujoco/tests/test_viewer.py b/motrix_deploy_mujoco/tests/test_viewer.py index eca539a0..b147529b 100644 --- a/motrix_deploy_mujoco/tests/test_viewer.py +++ b/motrix_deploy_mujoco/tests/test_viewer.py @@ -9,6 +9,7 @@ import numpy as np import pytest +from motrix_deploy.runtime.scheduler import FixedStepScheduler from motrix_deploy_mujoco.viewer import MujocoGlfwViewer from motrix_env_core.config.scene import SystemCameraCfg from motrix_env_core.input import KeyboardPlanarVelocityBinding @@ -298,10 +299,10 @@ def test_render_event_pump_interrupts_without_keyboard_binding(event) -> None: 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 control_helpers import DummyServoPolicyProcessor - from motrix_deploy.policy import NoOpPolicyRuntime - from motrix_deploy.runtime.control import ControlSession + from motrix_deploy.policy import NoOpPolicy + from motrix_deploy.runtime.control import ControlSession, PolicyController from motrix_deploy_mujoco import runtime from motrix_env_core.input import ConstantPlanarVelocityBinding @@ -312,20 +313,14 @@ def test_gui_runtime_pumps_startup_camera_and_close_with_constant_input( 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, + controller = PolicyController( + DummyServoPolicyProcessor(robot.spec, *control_gains), NoOpPolicy(robot.spec.joint_count), steps=10 ) waits = [] - class Scheduler: - overrun_count = 0 - + class Scheduler(FixedStepScheduler): def __init__(self, period_s): + super().__init__(period_s) assert period_s == simulation.control_period_s def reset(self): @@ -345,15 +340,21 @@ def deliver_events(): poll() monkeypatch.setattr(glfw, "poll_events", deliver_events) - simulation.bind_control_session(control) + session = ControlSession( + controller=controller, + robot=robot, + command_binding=ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)), + period_s=0.02, + state_timeout_s=0.1, + ) + simulation.bind_session(session) with simulation: assert glfw.poll_count == 1 and glfw.swap_count == 1 - result = simulation.run(steps=10) + result = simulation.run() assert result.exit_reason == "interrupted" and result.completed_steps == 3 - assert result.simulation_time_s == pytest.approx(0.06) + assert result.policy_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() diff --git a/motrix_deploy_tasks/README.md b/motrix_deploy_tasks/README.md index b06db9e5..22413295 100644 --- a/motrix_deploy_tasks/README.md +++ b/motrix_deploy_tasks/README.md @@ -1,7 +1,8 @@ # Motrix Deploy Tasks -`motrix-deploy-tasks` supplies the versioned Go2 walking task, flat/rough deployment worlds, -and installed Hydra recipes for `motrix-deploy`. It does not provide a simulation or hardware backend. +`motrix-deploy-tasks` supplies the versioned deployment tasks and installed Hydra recipes +for `motrix-deploy`: the Go2 walking task with flat/rough worlds, and the G1 WBT dance +task. It does not provide a simulation or hardware backend. Install a backend plugin and use an artifact exported for the matching task: @@ -9,9 +10,64 @@ Install a backend plugin and use an artifact exported for the matching task: motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy ``` -Use `go2-walk-rough/sim` for the rough scene, `runtime.backend=motrixsim` for native MotrixSim, -or `go2-walk-flat/hardware` for Unitree hardware. Recipes are packaged with the plugin; -no workspace config path is required. +Use `go2-walk-rough/sim` for the rough scene, `runtime.backend=motrixsim` for native +MotrixSim, or `go2-walk-flat/hardware` for Unitree hardware. Recipes are packaged with the +plugin; no workspace config path is required. See the [deployment tutorial](../docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md) for export, configuration, and hardware safety procedures. + +## G1 WBT dance: MuJoCo / MotrixSim sim2sim + +Use the standard **export → artifact → `motrix-deploy`** pipeline. The `g1_wbt/v1` task +embeds the actor's motion references, servo gains, action scales, preparation gains and +timing, and hardware-observable termination thresholds in the artifact. Deployment +requires neither Torch nor an RL environment; export needs the training provider and task +environment packages, but does **not** train. + +```bash +source .venv/bin/activate +RUN=runs/g1-wbt-dance/motrix/torch/fastsac/ +python scripts/export_deploy.py run="$RUN" \ + output=artifacts/g1-wbt-dance-software-pd.deploy validation.atol=3e-5 + +motrix-deploy task=g1-wbt-dance/sim \ + artifact=artifacts/g1-wbt-dance-software-pd.deploy +``` + +Headless validation and artifact inspection: + +```bash +motrix-deploy task=g1-wbt-dance/sim \ + artifact=artifacts/g1-wbt-dance-software-pd.deploy runtime.viewer=false +motrix-deploy inspect artifact=artifacts/g1-wbt-dance-software-pd.deploy +``` + +Notes: + +- The default duration comes from the embedded single clip; shorter runs use `duration_s`, + longer durations are rejected before opening the backend instead of looping or holding. +- No velocity command is needed: WBT motion is artifact-owned. +- The same artifact runs on MuJoCo and MotrixSim (`runtime.backend=motrixsim`, with + `runtime.physics.solver_iterations=3` matching the training budget). It is not a + hardware deployment recipe: the current Unitree transport is Go2-specific, and sim2sim + success is not evidence of hardware safety. +- The recipe's 0.002 s physics step (10 substeps per 0.02 s control tick) keeps the stiff + preparation PD inside MuJoCo's explicit-damping stability region. + +The task runs a four-phase lifecycle mirroring real deployment practice — `prepare` (ramp +measured joints to the robot's default standing pose with artifact-embedded gains and +timing, gated on continuously measured readiness), `policy_hold`, `playback`, `damping`. +The dance ends in a deliberately leaned pose that only the tracking policy can balance; +after playback the task enters measured damping directly. Ending upright requires the +motion clip itself to finish at the standing pose. +Takeover after measured readiness is automatic by default; programmatic applications can +stage it manually via `automatic_start=False`, `attach_keyboard`, or the request methods. +The CLI routes no task-specific options — operator input and preparation timing are task +semantics embedded in the artifact or set programmatically. See the +[deployment tutorial](../docs/source/en/user_guide/tutorial/advanced/motrix_deploy.md) +for the complete runtime, session, and controller contracts. + +```bash +python -m pytest motrix_deploy_tasks/tests -q +``` diff --git a/motrix_deploy_tasks/pyproject.toml b/motrix_deploy_tasks/pyproject.toml index d3cd6fc7..8285dba6 100644 --- a/motrix_deploy_tasks/pyproject.toml +++ b/motrix_deploy_tasks/pyproject.toml @@ -19,7 +19,8 @@ dependencies = [ ] [project.entry-points."motrix_deploy.tasks"] -"go2_walk/v1" = "motrix_deploy_tasks.tasks.go2_walk:Go2WalkDeployTaskV1" +"go2_walk/v1" = "motrix_deploy_tasks.tasks.go2_walk:Go2WalkDeployTask" +"g1_wbt/v1" = "motrix_deploy_tasks.tasks.g1_wbt:G1WbtDeployTask" [project.entry-points."motrix_deploy.envs"] flat = "motrix_deploy_tasks.envs.flat:build_scene" diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/g1-wbt-dance/sim.yaml b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/g1-wbt-dance/sim.yaml new file mode 100644 index 00000000..57d4bc11 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/g1-wbt-dance/sim.yaml @@ -0,0 +1,26 @@ +# @package _global_ +defaults: + - /runtime: sim + - _self_ + +# Task-owned default model: the compiler-validated 400k-iteration FastSAC dance +# checkpoint. Override with an explicit artifact=... to deploy another export. +artifact: artifacts/g1-wbt-dance.deploy + +runtime: + backend: mujoco + sensor_bindings: + base_angular_velocity: base_gyro + base_linear_acceleration: base_accel + base_linear_velocity: null + deploy_env_id: flat + robot_id: g1-29dof + physics: + # 0.002 s (10 substeps per 0.02 s control tick) keeps the stiff standing-hold + # preparation PD free of the discrete contact limit cycle seen at 0.005 s in + # MuJoCo; both backends complete the full four-phase flow at this dt. + dt: 0.002 + # Use runtime.physics.solver_iterations=3 to match MotrixSim training. + solver_iterations: 100 + +command: null diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/flat.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/flat.py index 20ccb359..9e6bd433 100644 --- a/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/flat.py +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/flat.py @@ -4,14 +4,28 @@ """A robot-free flat-ground deployment world.""" from motrix_env_core.config import configclass -from motrix_env_core.config.scene import FlatTerrainCfg, SceneCfg, SceneObjsCfg +from motrix_env_core.config.scene import FlatTerrainCfg, SceneCfg, SceneObjsCfg, SystemCameraCfg +from motrix_env_core.config.scene.base import SceneVisualCfg +from motrix_env_core.config.scene.light import LightCfg @configclass class FlatWorldObjsCfg(SceneObjsCfg): floor: FlatTerrainCfg = FlatTerrainCfg(friction=(0.6, 0.005, 0.0001)) + sun: LightCfg = LightCfg(color=(0.7, 0.7, 0.7), illuminance=10_000.0) def build_scene() -> SceneCfg: """Build fresh flat ground with an empty robot slot.""" - return SceneCfg(objs=FlatWorldObjsCfg()) + return SceneCfg( + objs=FlatWorldObjsCfg(), + # Front view matching the G1 WBT training scene (g1/common.py). + system_camera=SystemCameraCfg(distance=6.0, elevation=-20.0, azimuth=180.0), + 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, + tone_mapping="none", + ), + ) diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/rough.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/rough.py index 69bbede3..7321a33a 100644 --- a/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/rough.py +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/envs/rough.py @@ -11,6 +11,7 @@ SceneAssetsCfg, SceneCfg, SceneObjsCfg, + SystemCameraCfg, ) @@ -32,4 +33,9 @@ class RoughWorldObjsCfg(SceneObjsCfg): def build_scene() -> SceneCfg: """Build fresh seeded rough ground with an empty robot slot.""" - return SceneCfg(assets=RoughWorldAssetsCfg(), objs=RoughWorldObjsCfg()) + # Front view matching the quadruped training scenes (e.g. quadruped/cfg.py). + return SceneCfg( + assets=RoughWorldAssetsCfg(), + objs=RoughWorldObjsCfg(), + system_camera=SystemCameraCfg(distance=10.0, elevation=-25.0, azimuth=90.0), + ) diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/tasks/g1_wbt.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/tasks/g1_wbt.py new file mode 100644 index 00000000..77131631 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/tasks/g1_wbt.py @@ -0,0 +1,646 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""G1 WBT task data, policy I/O and measured-state deployment controllers.""" + +from __future__ import annotations + +from dataclasses import dataclass, field, replace +from typing import TYPE_CHECKING, Annotated, ClassVar + +import numpy as np +from pydantic import Field + +if TYPE_CHECKING: + from motrix_deploy.artifact.io import Artifact + +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.policy import Policy +from motrix_deploy.policy.processing import PolicyProcessor +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy.runtime.control import PolicyController +from motrix_deploy.runtime.lifecycle import ControllerStep +from motrix_deploy.task import DeployTask + +FiniteFloat = Annotated[float, Field(allow_inf_nan=False)] +NonNegativeFloat = Annotated[FiniteFloat, Field(ge=0)] +JointVector = Annotated[list[FiniteFloat], Field(min_length=29, max_length=29)] +GainVector = Annotated[list[NonNegativeFloat], Field(min_length=29, max_length=29)] +Quaternion = Annotated[list[FiniteFloat], Field(min_length=4, max_length=4)] + + +class G1WbtTaskSpec(TaskSpec): + """One motion clip in canonical robot order, inline or as an artifact NPZ payload.""" + + task_name: ClassVar[str] = "g1_wbt/v1" + period_s: Annotated[FiniteFloat, Field(gt=0)] = 0.02 + action_scale: GainVector + kp: GainVector + kd: GainVector + preparation_joint_position: JointVector | None = None + preparation_kp: GainVector | None = None + preparation_kd: GainVector | None = None + preparation_ramp_duration_s: NonNegativeFloat = 2.0 + preparation_settle_duration_s: NonNegativeFloat = 1.0 + preparation_timeout_s: Annotated[FiniteFloat, Field(gt=0)] = 5.0 + # The motion clip travels as the artifact's NPZ payload (joint_pos/joint_vel/ + # reference_quaternion_xyzw arrays); the task loads and validates it. + motion_frames: Annotated[int, Field(ge=1)] + reference_body_name: str = "torso_link" + termination_ref_orientation_threshold: NonNegativeFloat + termination_joint_position_threshold: NonNegativeFloat + termination_joint_velocity_threshold: NonNegativeFloat + + +@dataclass(frozen=True) +class G1WbtMotion: + """One validated motion clip in canonical robot order.""" + + joint_pos: FloatArray + joint_vel: FloatArray + reference_quaternion_xyzw: FloatArray + + def __post_init__(self) -> None: + joint_pos = np.asarray(self.joint_pos) + joint_vel = np.asarray(self.joint_vel) + quaternions = np.asarray(self.reference_quaternion_xyzw) + if joint_pos.ndim != 2 or joint_pos.shape[0] < 1 or joint_pos.shape[1] < 1: + raise ValidationError("motion.joint_pos", "a non-empty frame matrix", joint_pos.shape) + if joint_vel.shape != joint_pos.shape: + raise ValidationError("motion.joint_vel", f"shape {joint_pos.shape}", joint_vel.shape) + if quaternions.shape != (joint_pos.shape[0], 4): + raise ValidationError("motion.reference_quaternion_xyzw", f"({joint_pos.shape[0]}, 4)", quaternions.shape) + if not np.allclose(np.linalg.norm(quaternions, axis=1), 1, atol=1e-3, rtol=0): + raise ValidationError("motion.reference_quaternion_xyzw", "unit xyzw quaternions", None) + + def __len__(self) -> int: + return int(np.asarray(self.joint_pos).shape[0]) + + +def _load_motion(spec: G1WbtTaskSpec, artifact: Artifact | None) -> G1WbtMotion: + """Load and validate the motion clip from the artifact's single NPZ payload.""" + if artifact is None: + raise ValidationError("task.payloads", "the deployment artifact", None) + if len(artifact.manifest.payloads) != 1: + raise ValidationError("task.payloads", "exactly one motion payload entry", len(artifact.manifest.payloads)) + with np.load(artifact.root / artifact.manifest.payloads[0].path, allow_pickle=False) as arrays: + try: + motion = G1WbtMotion(arrays["joint_pos"], arrays["joint_vel"], arrays["reference_quaternion_xyzw"]) + except KeyError as error: + raise ValidationError( + f"task.payloads.{error.args[0]}", "a motion channel in the NPZ payload", None + ) from error + if len(motion) != spec.motion_frames: + raise ValidationError("task.motion_frames", f"{len(motion)} motion frames", spec.motion_frames) + return motion + + +class G1WbtPolicyProcessor(PolicyProcessor[None]): + """Measured-state G1 policy I/O with raw history and servo safety limits. + + The embedded spec is already in canonical robot order (the profile compiler + performs name-based motion remapping at export), so the actor math below + indexes motion channels directly. The actor has 154 inputs: reference joint + positions (29), reference joint velocities (29), relative torso orientation + (6), body-frame base angular velocity (3), measured joint positions minus + default (29), measured joint velocities (29), and the last raw policy action + (29). ``action_scale`` is the *resolved* scale in radians per unit action: + training WBT uses ``0.25 * effort_limit / kp`` (zero for zero kp), not a + uniform 0.25-radian scale. No noise, clipping or frame advancement happens + in the observation; ``previous_action`` history stores the raw action. + """ + + observation_size = 154 + action_size = 29 + + def __init__(self, spec: G1WbtTaskSpec, robot: RobotSpec, motion: G1WbtMotion) -> None: + if robot.joint_count != 29: + raise ValidationError("robot.joint_count", 29, robot.joint_count) + if robot.base_link_name != "pelvis" or spec.reference_body_name != "torso_link": + raise ValidationError("g1_wbt.kinematics", "pelvis base and torso_link reference", spec.reference_body_name) + self.spec = spec + self.robot = robot + self.motion = motion + # G1 model: aligned pelvis IMU, identity fixed rotations, serial Z-X-Y waist. + # Inertial-frame quaternions are NOT link-frame mounting transforms. + try: + self._waist_indices = np.asarray( + [robot.joint_names.index(f"waist_{axis}_joint") for axis in ("yaw", "roll", "pitch")] + ) + except ValueError as error: + raise ValidationError("robot.joint_names", "G1 yaw/roll/pitch waist joints", robot.joint_names) from error + self._frames = len(motion) + self._motion_joint_position = np.asarray(motion.joint_pos, dtype=np.float32) + self._motion_joint_velocity = np.asarray(motion.joint_vel, dtype=np.float32) + self._reference_quaternion = np.asarray(motion.reference_quaternion_xyzw, dtype=np.float32) + self._default_joint_position = robot.default_joint_position.astype(np.float32, copy=True) + self._action_scale = np.asarray(spec.action_scale, dtype=np.float32) + self._previous_action = np.zeros(self.action_size, dtype=np.float32) + self._kp = np.asarray(spec.kp, dtype=np.float32) + self._kd = np.asarray(spec.kd, dtype=np.float32) + self._latest_frame = 0 + self._reference_frame: int | None = None + + def reset_reference(self) -> None: + """Restore the initial reference when starting a new task execution.""" + self._reference_frame = 0 + self._latest_frame = 0 + + def set_reference_frame(self, frame: int) -> None: + """Select a reference independently of the continuously running policy.""" + if not 0 <= frame < self._frames: + raise ValidationError("reference_frame", "a frame inside the single motion clip", frame) + self._reference_frame = frame + + def validate_command(self, command: None) -> None: + if command is not None: + raise ValidationError("command", "None (motion is artifact-owned)", type(command).__name__) + + def reset(self, state: RobotState, context: ControlContext[None]) -> None: + self._previous_action.fill(0) + self._latest_frame = 0 + + @property + def previous_action(self) -> np.ndarray: + """Last processed raw action in policy order, independently owned.""" + return self._previous_action.copy() + + def actor_observation( + self, + frame: int, + torso_quaternion_xyzw: np.ndarray, + base_angular_velocity_body: np.ndarray, + joint_positions: np.ndarray, + joint_velocities: np.ndarray, + previous_action: np.ndarray | None = None, + ) -> np.ndarray: + """Return the raw actor vector for a caller-selected motion frame. + + Torso and motion quaternions are world-frame xyzw. The relative rotation + is ``R(torso).T @ R(reference)``; its first TWO ROWS are flattened in + row-major order (not the commonly used first-two-columns encoding). + Base angular velocity is already expressed in the floating-base frame, + not the torso frame. Explicit ``previous_action`` overrides the history + for this observation only, without mutating it. Otherwise the most recent + ``process_action`` input is echoed, as training observes the current raw + action after stepping and thus the previous action at the next inference. + """ + relative_rotation = _rotation_matrix(torso_quaternion_xyzw).T @ _rotation_matrix( + self._reference_quaternion[frame] + ) + raw_history = self._previous_action if previous_action is None else previous_action + return np.concatenate( + ( + self._motion_joint_position[frame], + self._motion_joint_velocity[frame], + relative_rotation[:2].reshape(-1), + base_angular_velocity_body, + np.asarray(joint_positions) - self._default_joint_position, + joint_velocities, + raw_history, + ) + ).astype(np.float32) + + def torso_orientation(self, state: RobotState) -> FloatArray: + """World XYZW torso attitude from measured pelvis attitude and waist FK. + + This fixed G1-29DOF chain matches the deployed model, not arbitrary robots. + Hardware must calibrate its IMU into the pelvis frame and map serial joint + coordinates before constructing RobotState. No simulator body query, + root position, desired joint position or motion quaternion is used. + """ + orientation = state.base_orientation_xyzw.astype(np.float64) + yaw, roll, pitch = state.joint_position[self._waist_indices] + for angle, axis in zip((yaw, roll, pitch), (2, 0, 1)): + rotation = np.zeros(4, dtype=np.float64) + rotation[axis] = np.sin(float(angle) / 2) + rotation[3] = np.cos(float(angle) / 2) + v, w = orientation[:3], orientation[3] + r, s = rotation[:3], rotation[3] + orientation = np.concatenate((w * r + s * v + np.cross(v, r), [w * s - np.dot(v, r)])) + return (orientation / np.linalg.norm(orientation)).astype(np.float32) + + def build_observation(self, state: RobotState, context: ControlContext[None]) -> FloatArray: + frame = context.step if self._reference_frame is None else self._reference_frame + if not 0 <= frame < self._frames: + raise ValidationError("context.step", "a frame inside the single motion clip", frame) + self._latest_frame = frame + return self.actor_observation( + frame, + self.torso_orientation(state), + state.base_angular_velocity, + state.joint_position, + state.joint_velocity, + ) + + def process_action(self, action: FloatArray) -> JointServoCommand: + raw = float32_array(action, path="action.raw", shape=(29,)) + self._previous_action = raw.copy() # Boundary arrays are read-only; history must stay writable. + # Neither the raw action nor the scaled target is clipped before the + # robot limit clamp below: this matches the training joint-position term. + target = self._default_joint_position + self._action_scale * raw + target = np.clip(target, self.robot.position_lower, self.robot.position_upper).astype(np.float32) + zeros = np.zeros(29, dtype=np.float32) + return JointServoCommand(target, zeros, zeros, self._kp, self._kd) + + def check_termination(self, state: RobotState) -> str | None: + frame = self._latest_frame + torso = self.torso_orientation(state) + reference = self._reference_quaternion[frame] + gravity_error = abs(2 * (reference[0] ** 2 + reference[1] ** 2 - torso[0] ** 2 - torso[1] ** 2)) + if gravity_error > self.spec.termination_ref_orientation_threshold: + return "bad_ref_ori" + violation = np.maximum(self.robot.position_lower - state.joint_position, 0) + np.maximum( + state.joint_position - self.robot.position_upper, 0 + ) + if np.max(violation) > self.spec.termination_joint_position_threshold: + return "bad_dof_pos" + if np.any(np.abs(state.joint_velocity) > self.spec.termination_joint_velocity_threshold): + return "bad_dof_vel" + return None + + +def _rotation_matrix(quaternion_xyzw: np.ndarray) -> np.ndarray: + """Quaternion matrix with the same norm correction as training's encoder.""" + x, y, z, w = np.asarray(quaternion_xyzw, dtype=np.float64) + s = 2.0 / (x * x + y * y + z * z + w * w) + return np.array( + [ + [1 - s * (y * y + z * z), s * (x * y - z * w), s * (x * z + y * w)], + [s * (x * y + z * w), 1 - s * (x * x + z * z), s * (y * z - x * w)], + [s * (x * z - y * w), s * (y * z + x * w), 1 - s * (x * x + y * y)], + ] + ) + + +@dataclass +class StageResult: + """G1 controller-local readiness or damping diagnostics.""" + + name: str + steps: int = 0 + time_s: float = 0.0 + complete: bool = False + metrics: dict[str, float] = field(default_factory=dict) + error: str | None = None + + +@dataclass(frozen=True) +class G1PreparationSettings: + """Measured readiness limits; preparation never injects a reference root state.""" + + ramp_duration_s: float = 2.0 + settle_duration_s: float = 1.0 + timeout_s: float = 5.0 + max_joint_error_rad: float = 0.2 + max_joint_velocity_rad_s: float = 1.0 + max_base_tilt_rad: float = 0.5 + + +class G1ReadyController: + """Ramp from measured joints, then require an uninterrupted ready interval.""" + + name = "prepare" + + def __init__( + self, + target: FloatArray, + kp: FloatArray, + kd: FloatArray, + robot: RobotSpec, + settings: G1PreparationSettings, + ) -> None: + self._initial_position: FloatArray | None = None + self._target = np.clip(target, robot.position_lower, robot.position_upper).astype(np.float32) + self._kp = kp + self._kd = kd + self._robot = robot + self._settings = settings + self._stop_requested = False + self._ready_since: float | None = None + self._reached_ready = False + self._zeros = np.zeros(robot.joint_count, dtype=np.float32) + + def reset(self, state: RobotState) -> None: + self._initial_position = state.joint_position.copy() + self._ready_since = None + self._reached_ready = False + self._stop_requested = False + + def hold_command(self) -> JointServoCommand: + """Keep the independent standing posture while waiting for a start request.""" + return JointServoCommand(self._target, self._zeros, self._zeros, self._kp, self._kd) + + def request_stop(self) -> None: + self._stop_requested = True + + def step(self, state: RobotState, context: ControlContext[None]) -> ControllerStep: + if self._stop_requested: + return ControllerStep(complete=True) + elapsed_time_s = context.elapsed_time_s + assert self._initial_position is not None + settings = self._settings + x, y, _, _ = state.base_orientation_xyzw + tilt = float(np.arccos(np.clip(1 - 2 * (float(x) ** 2 + float(y) ** 2), -1, 1))) + metrics = { + "max_joint_error_rad": float(np.max(np.abs(state.joint_position - self._target))), + "max_joint_velocity_rad_s": float(np.max(np.abs(state.joint_velocity))), + "base_tilt_rad": tilt, + } + if tilt > settings.max_base_tilt_rad: + return ControllerStep(metrics=metrics, error=f"unsafe base tilt during preparation: {metrics}") + ready = ( + elapsed_time_s + 1e-9 >= settings.ramp_duration_s + and metrics["max_joint_error_rad"] <= settings.max_joint_error_rad + and metrics["max_joint_velocity_rad_s"] <= settings.max_joint_velocity_rad_s + ) + if ready: + if self._ready_since is None: + self._ready_since = elapsed_time_s + else: + self._ready_since = None + if ( + self._ready_since is not None + and elapsed_time_s - self._ready_since + 1e-9 >= settings.settle_duration_s + and (self._reached_ready or elapsed_time_s <= settings.timeout_s + 1e-9) + ): + self._reached_ready = True + return ControllerStep(complete=True, metrics=metrics) + if not self._reached_ready and elapsed_time_s + 1e-9 >= settings.timeout_s: + return ControllerStep(metrics=metrics, error=f"preparation timed out: {metrics}") + fraction = ( + np.clip((elapsed_time_s + context.dt_s) / settings.ramp_duration_s, 0, 1) + if settings.ramp_duration_s > 0 + else 1 + ) + target = self._initial_position + np.float32(fraction) * (self._target - self._initial_position) + target = np.clip(target, self._robot.position_lower, self._robot.position_upper).astype(np.float32) + return ControllerStep( + command=JointServoCommand(target, self._zeros, self._zeros, self._kp, self._kd), metrics=metrics + ) + + +class G1DampingController: + """Damp measured joint velocity briefly, without holding any reference pose.""" + + name = "stop" + + def __init__(self, kd: FloatArray, robot: RobotSpec) -> None: + self._kd = kd + self._robot = robot + self._zeros = np.zeros_like(kd) + self._stop_requested = False + + def reset(self, state: RobotState) -> None: + self._stop_requested = False + + def request_stop(self) -> None: + self._stop_requested = True + + def step(self, state: RobotState, context: ControlContext[None]) -> ControllerStep: + elapsed_time_s = context.elapsed_time_s + if self._stop_requested or elapsed_time_s + 1e-9 >= 0.3: + return ControllerStep(complete=True) + # The backend validates position targets even with kp=0. Clipping keeps + # the command valid but cannot add a position-holding torque. + target = np.clip(state.joint_position, self._robot.position_lower, self._robot.position_upper).astype( + np.float32 + ) + return ControllerStep(command=JointServoCommand(target, self._zeros, self._zeros, self._zeros, self._kd)) + + +class G1WbtDeployTask(DeployTask[None]): + """Own standing preparation, policy hold, motion playback and damping stop.""" + + spec_type = G1WbtTaskSpec + + def __init__( + self, + spec: G1WbtTaskSpec, + robot: RobotSpec, + policy: Policy, + steps: int | None = None, + *, + automatic_start: bool = True, + motion: G1WbtMotion | None = None, + artifact: Artifact | None = None, + ): + if motion is None: + motion = _load_motion(spec, artifact) + budget = len(motion) if steps is None else steps + if not 0 < budget <= len(motion): + raise ValidationError("steps", "a positive budget inside the single motion clip", budget) + self.spec = spec + self.robot = robot + self.motion = motion + self.policy_io = G1WbtPolicyProcessor(spec, robot, motion) + self.policy_controller = PolicyController(self.policy_io, policy) + self._playback_budget = budget + self._standing_position = np.asarray( + robot.default_joint_position + if spec.preparation_joint_position is None + else spec.preparation_joint_position, + dtype=np.float32, + ).copy() + self._preparation_kp = np.asarray( + spec.kp if spec.preparation_kp is None else spec.preparation_kp, dtype=np.float32 + ) + self._preparation_kd = np.asarray( + spec.kd if spec.preparation_kd is None else spec.preparation_kd, dtype=np.float32 + ) + self._preparation_settings = G1PreparationSettings( + ramp_duration_s=spec.preparation_ramp_duration_s, + settle_duration_s=spec.preparation_settle_duration_s, + timeout_s=spec.preparation_timeout_s, + ) + self.ready = G1ReadyController( + self._standing_position, self._preparation_kp, self._preparation_kd, robot, self._preparation_settings + ) + self.damping = G1DampingController(self.policy_io._kd, robot) + self.transitions: list[StageResult] = [] + self._phase = "prepare" + self._record: StageResult | None = None + self._stop_requested = False + self._policy_start_requested = False + self._motion_start_requested = False + self._preparation_ready = False + self._playback_steps = 0 + self._stage_origin_s: float | None = None + self._keyboard = None + # Default takeover mode: after measured readiness the policy starts by + # itself and playback follows the task's single mandatory frozen-reference + # interval. Programmatic applications pass automatic_start=False (manual + # request methods only) or attach a keyboard for operator staging. + self._automatic_hold_ticks = 0 if automatic_start else None + self._held_ticks = 0 + + @property + def phase(self) -> str: + return self._phase + + @property + def preparation_ready(self) -> bool: + return self._preparation_ready + + @property + def playback_steps(self) -> int: + return self._playback_steps + + @classmethod + def default_duration_s(cls, spec: G1WbtTaskSpec) -> float: + return spec.motion_frames * spec.period_s + + def validate_command(self, command: None) -> None: + self.policy_io.validate_command(command) + + def _enter_phase(self, phase: str, *, completed: bool = True) -> None: + if self._record is not None: + self._record.complete = completed + self._phase = phase + self._stage_origin_s = None + self._record = StageResult(name=phase) + self.transitions.append(self._record) + + def reset(self, state: RobotState) -> None: + self._stop_requested = False + self._policy_start_requested = False + self._motion_start_requested = False + self._preparation_ready = False + self._playback_steps = 0 + self._held_ticks = 0 + self.policy_controller.steps = None + self.policy_io.reset_reference() + self.transitions.clear() + self._record = None + self._enter_phase("prepare") + self.ready.reset(state) + + def request_policy_start(self) -> None: + """Latch operator intent; takeover still requires measured preparation readiness.""" + self._policy_start_requested = True + + def request_motion_start(self) -> None: + """Latch playback intent without changing control state from the input thread.""" + self._motion_start_requested = True + + def request_stop(self) -> None: + self._stop_requested = True + + def attach_keyboard(self, keyboard, *, hold_ticks: int = 0) -> None: + """Let the task translate viewer keyboard press edges into phase requests.""" + if keyboard is None: + raise ValidationError("operator.keyboard", "an event-framed keyboard device", None) + self._keyboard = keyboard + self._automatic_hold_ticks = hold_ticks + + def enable_automatic_start(self, hold_ticks: int = 0) -> None: + """Let the task script its own phase requests after measured readiness.""" + self._keyboard = None + self._automatic_hold_ticks = hold_ticks + + @property + def operator_hint(self) -> str: + """One-line key reference printed by the CLI when keyboard input is attached.""" + return "p start policy m start motion o stop" + + def _process_operator_inputs(self) -> None: + if self._keyboard is not None: + self._keyboard.poll() + # Letters only: MotrixSim's viewer input supports letters, function, + # special and arrow keys, but not punctuation such as ']'. + if self._keyboard.is_key_down("p"): + self._policy_start_requested = True + if self._keyboard.is_key_down("m"): + self._motion_start_requested = True + if self._keyboard.is_key_down("o"): + self._stop_requested = True + elif self._automatic_hold_ticks is None: + return + elif self._phase == "prepare": + if self._preparation_ready: + self._policy_start_requested = True + return + elif self._phase != "policy_hold": + return + # Once the policy runs, playback follows the configured hold target; an + # explicit `m` press merely starts it earlier. + self._held_ticks += 1 + if self._held_ticks >= self._automatic_hold_ticks: + self._motion_start_requested = True + + def _begin_damping(self, state: RobotState) -> None: + self._enter_phase("damping", completed=not self._stop_requested) + self.damping.reset(state) + + def _record_decision(self, decision: ControllerStep, context: ControlContext[None]) -> None: + assert self._record is not None + if self._stage_origin_s is None: + self._stage_origin_s = context.elapsed_time_s + record = self._record + record.time_s = context.elapsed_time_s - self._stage_origin_s + record.metrics = dict(decision.metrics) + if decision.error: + record.error = decision.error + elif decision.command is not None: + record.steps += 1 + record.time_s += context.dt_s + + def _deterministic_step(self, state: RobotState, context: ControlContext[None]) -> ControllerStep: + assert self._record is not None + if self._stage_origin_s is None: + self._stage_origin_s = context.elapsed_time_s + local = ControlContext( + self._record.steps, context.elapsed_time_s - self._stage_origin_s, context.command, context.dt_s + ) + controller = self.ready if self._phase == "prepare" else self.damping + decision = controller.step(state, local) + if self._phase == "prepare": + self._preparation_ready = decision.complete and decision.error is None + if decision.complete and not self._policy_start_requested: + decision = replace(decision, complete=False, command=self.ready.hold_command()) + self._record_decision(decision, context) + return replace(decision, success=not self._stop_requested) + + def step(self, state: RobotState, context: ControlContext[None]) -> ControllerStep: + self._process_operator_inputs() + if self._phase == "complete": + return ControllerStep(complete=True, success=not self._stop_requested) + if self._stop_requested and self._phase != "damping": + self._begin_damping(state) + if self._phase == "prepare": + decision = self._deterministic_step(state, context) + if decision.error is not None or not (self._preparation_ready and self._policy_start_requested): + return decision + self.policy_controller.reset(state) + self._enter_phase("policy_hold") + if self._phase == "policy_hold": + # Policy takeover always gets at least one held-reference interval before playback. + assert self._record is not None + if self._motion_start_requested and self._record.steps > 0: + self._enter_phase("playback") + # Preserve inference history; only the remaining playback budget becomes finite. + self.policy_controller.steps = self.policy_controller.completed_steps + self._playback_budget + if self._phase in {"policy_hold", "playback"}: + frame = self._playback_steps if self._phase == "playback" else 0 + self.policy_io.set_reference_frame(min(frame, len(self.motion) - 1)) + decision = self.policy_controller.step(state, context) + self._record_decision(decision, context) + if not decision.complete: + if self._phase == "playback" and decision.policy_tick: + self._playback_steps += 1 + return decision + self._begin_damping(state) + decision = self._deterministic_step(state, context) + if decision.complete: + assert self._record is not None + self._record.complete = True + self._phase = "complete" + return decision 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 index 9e453b78..588c23bb 100644 --- a/motrix_deploy_tasks/src/motrix_deploy_tasks/tasks/go2_walk.py +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/tasks/go2_walk.py @@ -11,7 +11,11 @@ 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.policy import Policy +from motrix_deploy.policy.processing import PolicyProcessor +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy.runtime.control import PolicyController +from motrix_deploy.runtime.lifecycle import ControllerStep from motrix_deploy.task import DeployTask from motrix_env_core.input import PlanarVelocityCommand @@ -45,7 +49,6 @@ class Go2WalkTaskSpec(TaskSpec): 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": @@ -60,16 +63,12 @@ def validate_joint_vectors_and_ranges(self) -> "Go2WalkTaskSpec": return self -class Go2WalkDeployTaskV1(DeployTask[PlanarVelocityCommand]): +class Go2WalkPolicyProcessor(PolicyProcessor[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( @@ -89,7 +88,6 @@ def __init__(self, spec: Go2WalkTaskSpec, robot: RobotSpec) -> None: 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 @@ -105,19 +103,14 @@ def check_termination(self, state: RobotState) -> str | None: 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: + def reset(self, state: RobotState, context: ControlContext[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.""" + def build_observation(self, state: RobotState, context: ControlContext[PlanarVelocityCommand]) -> FloatArray: + """Consume runtime-validated state and PolicyController-validated input commands.""" velocity = context.command.values[0, :] phase = np.zeros(4, dtype=np.float32) if context.step > 0 and np.linalg.norm(velocity) >= self._standing_threshold: @@ -153,7 +146,7 @@ def process_action(self, action: FloatArray) -> JointServoCommand: kd=self._kd, ) - def validate_command(self, command: PlanarVelocityCommand) -> None: + def validate_command(self, command: PlanarVelocityCommand | None) -> None: if not isinstance(command, PlanarVelocityCommand): raise ValidationError("command.type", "PlanarVelocityCommand", type(command).__name__) if command.batch_size != 1: @@ -178,3 +171,37 @@ def _projected_gravity(orientation_xyzw: FloatArray) -> FloatArray: dtype=np.float32, ) return rotation @ np.array([0.0, 0.0, -1.0], dtype=np.float32) + + +class Go2WalkDeployTask(DeployTask[PlanarVelocityCommand]): + """Complete Go2 walking task; its single policy phase handles normal stopping.""" + + spec_type = Go2WalkTaskSpec + + def __init__( + self, spec: Go2WalkTaskSpec, robot: RobotSpec, policy: Policy, steps: int | None = None, artifact=None + ): + self.spec = spec + self.robot = robot + self.policy_io = Go2WalkPolicyProcessor(spec, robot) + self.policy_controller = PolicyController(self.policy_io, policy, steps=steps) + + @property + def command_lower(self) -> FloatArray: + return self.policy_io.command_lower + + @property + def command_upper(self) -> FloatArray: + return self.policy_io.command_upper + + def validate_command(self, command: PlanarVelocityCommand | None) -> None: + self.policy_io.validate_command(command) + + def reset(self, state: RobotState) -> None: + self.policy_controller.reset(state) + + def step(self, state: RobotState, context: ControlContext[PlanarVelocityCommand]) -> ControllerStep: + return self.policy_controller.step(state, context) + + def request_stop(self) -> None: + self.policy_controller.request_stop() diff --git a/motrix_deploy_tasks/tests/test_cli.py b/motrix_deploy_tasks/tests/test_cli.py index d959bc8a..fbb5b20b 100644 --- a/motrix_deploy_tasks/tests/test_cli.py +++ b/motrix_deploy_tasks/tests/test_cli.py @@ -26,6 +26,7 @@ sha256_bytes, write_artifact, ) +from motrix_deploy.artifact.schema import PayloadSpec from motrix_deploy.contracts import RobotSpec, TensorSpec from motrix_deploy.env import assemble_deploy_scene, create_deploy_env from motrix_deploy_tasks.tasks.go2_walk import Go2WalkTaskSpec @@ -200,6 +201,7 @@ def test_installed_go2_task_can_be_created_without_preimport(tmp_path: Path) -> "from motrix_deploy.artifact import TaskSpec; " "from motrix_deploy.contracts import RobotSpec; " "from motrix_deploy.task import create_task; " + "from motrix_deploy.policy import NoOpPolicy; " "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) " @@ -208,9 +210,9 @@ def test_installed_go2_task_can_be_created_without_preimport(tmp_path: Path) -> "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'])); " + "task = create_task(spec, RobotSpec(**specs['robot']), NoOpPolicy(12)); " "assert type(task).__module__ == 'motrix_deploy_tasks.tasks.go2_walk'; " - "assert type(task).__name__ == 'Go2WalkDeployTaskV1'" + "assert type(task).__name__ == 'Go2WalkDeployTask'" ) result = subprocess.run( @@ -329,6 +331,64 @@ def test_headless_simulation_cli_runs_deterministic_onnx_fixture(tmp_path: Path, assert "duration_s" in invalid_bound.stderr +@pytest.mark.parametrize("backend,solver_iterations", [("mujoco", 100), ("motrixsim", 3)]) +def test_standard_cli_completes_four_phase_flow(tmp_path, backend, solver_iterations): + """Standing preparation, frozen-reference hold, then the requested playback budget.""" + pytest.importorskip(backend) + pytest.importorskip("onnxruntime") + from motrix_envs.deploy.g1_wbt import build_g1_wbt_profile + + profile = build_g1_wbt_profile("g1-wbt-dance") + graph = helper.make_graph( + [helper.make_node("MatMul", ["obs", "weight"], ["actions"])], + "zero_wbt_policy", + [helper.make_tensor_value_info("obs", TensorProto.FLOAT, [1, 154])], + [helper.make_tensor_value_info("actions", TensorProto.FLOAT, [1, 29])], + [numpy_helper.from_array(np.zeros((154, 29), dtype=np.float32), name="weight")], + ) + model = helper.make_model(graph, opset_imports=[helper.make_opsetid("", 17)]) + model.ir_version = 10 + payload = model.SerializeToString() + manifest = DeploymentManifest( + schema_version="motrix-deploy/v1", + source=SourceSpec("test", "wbt-smoke", "fixture"), + policy=PolicySpec( + "onnx/v1", + "policy/model.onnx", + sha256_bytes(payload), + TensorSpec("obs", (1, 154)), + TensorSpec("actions", (1, 29)), + ), + robot=profile.robot, + task=profile.task, + control=profile.control, + payloads=tuple(PayloadSpec(path, sha256_bytes(data)) for path, data in sorted(profile.payloads.items())), + ) + artifact = tmp_path / "wbt.deploy" + write_artifact(artifact, manifest, {"policy/model.onnx": payload, **profile.payloads}) + command = [ + sys.executable, + "-c", + "from motrix_deploy.cli import main; main()", + "task=g1-wbt-dance/sim", + f"artifact={artifact}", + "runtime.viewer=false", + f"runtime.backend={backend}", + f"runtime.physics.solver_iterations={solver_iterations}", + # The default zero hold plus the task's mandatory held-reference interval + # make the takeover-to-playback distance deterministic. + "duration_s=0.04", + ] + result = subprocess.run(command, check=False, capture_output=True, text=True) + assert result.returncode == 0, result.stderr + result.stdout + report = _rollout_result(result.stdout) + assert report["success"] + assert report["exit_reason"] == "completed" + # One mandatory hold interval plus the two-tick playback budget. + assert report["completed_steps"] == 3 + assert report["policy_simulation_time_s"] == pytest.approx(3 * profile.control.period_s) + + def _robot_spec() -> RobotSpec: return RobotSpec( base_link_name="base", @@ -366,7 +426,6 @@ def _task_spec() -> Go2WalkTaskSpec: 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, diff --git a/motrix_deploy_tasks/tests/test_g1_wbt.py b/motrix_deploy_tasks/tests/test_g1_wbt.py new file mode 100644 index 00000000..010c3241 --- /dev/null +++ b/motrix_deploy_tasks/tests/test_g1_wbt.py @@ -0,0 +1,140 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Focused, simulator-free tests of the current G1 WBT actor contract.""" + +import numpy as np +import pytest + +from motrix_deploy.contracts import RobotSpec, RobotState +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy_tasks.tasks.g1_wbt import G1WbtMotion, G1WbtPolicyProcessor, G1WbtTaskSpec + + +def _state(default): + return RobotState( + 0, + 0, + default, + np.zeros(29, np.float32), + np.array([0, 0, 0, 1], np.float32), + np.array([1, 2, 3], np.float32), + np.zeros(3, np.float32), + ) + + +def _motion(joint_pos=None, joint_vel=None, reference=None): + return G1WbtMotion( + joint_pos if joint_pos is not None else np.arange(58, dtype=np.float32).reshape(2, 29), + joint_vel if joint_vel is not None else np.arange(58, dtype=np.float32).reshape(2, 29) + 100, + np.asarray(reference if reference is not None else [[0, 0, 0, 1], [0, 0, 1, 0]], np.float32), + ) + + +def _spec(scale): + return G1WbtTaskSpec( + action_scale=[scale] * 29 if np.isscalar(scale) else scale.tolist(), + kp=[10] * 29, + kd=[1] * 29, + motion_frames=2, + termination_ref_orientation_threshold=0.4, + termination_joint_position_threshold=0.5, + termination_joint_velocity_threshold=10, + ) + + +def _io(scale=0.25, spec=None, motion=None): + default = np.linspace(-0.4, 0.7, 29, dtype=np.float32) + robot = RobotSpec( + "pelvis", + tuple(["waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint", *[f"joint_{i}" for i in range(26)]]), + default, + np.full(29, -1000, np.float32), # Servo limits far away: these tests assert unclamped math. + np.full(29, 1000, np.float32), + np.full(29, 10, np.float32), + ) + motion = motion if motion is not None else _motion() + return G1WbtPolicyProcessor(spec if spec is not None else _spec(scale), robot, motion), default + + +def _observation(io, default, frame=0, previous_action=None): + return io.actor_observation( + frame, + np.array([0, 0, 0, 1]), + np.array([1, -2, 3]), + default, + np.zeros(29), + previous_action, + ) + + +def test_observation_order_frame_selection_and_unscaled_terms(): + spec = _spec(0.25) + io, default = _io(spec=spec) + motion = io.motion + positions = default + np.arange(29, dtype=np.float32) / 10 + velocities = np.linspace(-20, 30, 29) + previous = np.linspace(-5, 4, 29) + angular_velocity = np.array([7, -8, 9]) + observation = io.actor_observation(1, np.array([0, 0, 0, 1]), angular_velocity, positions, velocities, previous) + # 154 and these boundaries are the deployed actor contract, not tuning. + assert observation.shape == (154,) + assert observation.dtype == np.float32 + np.testing.assert_array_equal(observation[:29], motion.joint_pos[1]) + np.testing.assert_array_equal(observation[29:58], motion.joint_vel[1]) + # Frame-1 reference [0,0,1,0] is a 180° Z rotation: relative = diag(-1,-1,1). + np.testing.assert_array_equal(observation[58:64], [-1, 0, 0, 0, -1, 0]) + np.testing.assert_array_equal(observation[64:67], angular_velocity) + np.testing.assert_allclose(observation[67:96], positions - default) + np.testing.assert_allclose(observation[96:125], velocities) + np.testing.assert_allclose(observation[125:], previous) + np.testing.assert_array_equal(io.previous_action, np.zeros(29)) + + +def test_noncommuting_relative_orientation_first_two_rows_xyzw(): + torso_angle, reference_angle = 0.7, -1.1 + reference = [0, np.sin(reference_angle / 2), 0, np.cos(reference_angle / 2)] + io, default = _io(spec=_spec(0.25), motion=_motion(reference=[[0, 0, 0, 1], reference])) + torso = np.array([np.sin(torso_angle / 2), 0, 0, np.cos(torso_angle / 2)]) + ct, st = np.cos(torso_angle), np.sin(torso_angle) + cr, sr = np.cos(reference_angle), np.sin(reference_angle) + torso_matrix = np.array([[1, 0, 0], [0, ct, -st], [0, st, ct]]) + reference_matrix = np.array([[cr, 0, sr], [0, 1, 0], [-sr, 0, cr]]) + relative = torso_matrix.T @ reference_matrix + actual = io.actor_observation(1, torso, np.zeros(3), default, np.zeros(29))[58:64] + np.testing.assert_allclose(actual, relative[:2].reshape(-1), atol=1e-7) + assert not np.allclose(actual, relative[:, :2].reshape(-1)) + assert not np.allclose(actual, (reference_matrix @ torso_matrix.T)[:2].reshape(-1)) + assert not np.allclose(actual, relative.T[:2].reshape(-1)) + # Quaternion sign and small normalization differences do not change rotation. + negated = io.actor_observation(1, -torso * 1.0001, np.zeros(3), default, np.zeros(29))[58:64] + np.testing.assert_allclose(negated, actual, atol=1e-7) + + +@pytest.mark.parametrize("scale", [0.31, np.linspace(0, 0.8, 29, dtype=np.float32)]) +def test_action_offset_scale_raw_history_without_clipping_and_reset(scale): + io, default = _io(scale) + np.testing.assert_array_equal(_observation(io, default)[125:], np.zeros(29)) + raw = np.linspace(-9, 8, 29, dtype=np.float32) + expected_raw = raw.copy() + target = io.process_action(raw).joint_position + assert target.dtype == np.float32 + np.testing.assert_allclose(target, default + scale * expected_raw) + # History is raw, not scaled target, not clamped action, and not caller-owned. + raw.fill(99) + target.fill(99) + np.testing.assert_array_equal(io.previous_action, expected_raw) + np.testing.assert_array_equal(_observation(io, default)[125:], expected_raw) + io.previous_action.fill(99) + np.testing.assert_array_equal(io.previous_action, expected_raw) + override = np.full(29, -3, dtype=np.float32) + np.testing.assert_array_equal(_observation(io, default, previous_action=override)[125:], override) + np.testing.assert_array_equal(io.previous_action, expected_raw) + next_raw = np.full(29, 2, dtype=np.float32) + io.process_action(next_raw) + np.testing.assert_array_equal(_observation(io, default)[125:], next_raw) + io.reset( + _state(default), + ControlContext(0, 0.0, None, dt_s=0.02), + ) + np.testing.assert_array_equal(_observation(io, default)[125:], np.zeros(29)) diff --git a/motrix_deploy_tasks/tests/test_g1_wbt_measured_fk.py b/motrix_deploy_tasks/tests/test_g1_wbt_measured_fk.py new file mode 100644 index 00000000..3e9cbb80 --- /dev/null +++ b/motrix_deploy_tasks/tests/test_g1_wbt_measured_fk.py @@ -0,0 +1,191 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Measured pelvis IMU + named waist FK matches independent simulator truth.""" + +from dataclasses import replace + +import numpy as np +import pytest +from motrix_robots.unitree import UnitreeG129Dof + +from motrix_deploy.contracts import RobotState +from motrix_deploy.runtime.config import ( + SensorBindings, + SimulationRuntimeConfig, +) +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy.runtime.factory import create_simulation_runtime +from motrix_deploy_tasks.tasks.g1_wbt import G1WbtMotion, G1WbtPolicyProcessor +from motrix_env_core.config import configclass +from motrix_env_core.config.scene import FlatTerrainCfg, SceneCfg, SceneObjsCfg +from motrix_env_core.config.sim import SimCfg +from motrix_envs.deploy.g1_wbt import build_g1_wbt_profile + + +@configclass +class _FlatG1Objs(SceneObjsCfg): + floor: FlatTerrainCfg = FlatTerrainCfg() + + +@pytest.fixture(scope="module") +def profile(): + return build_g1_wbt_profile("g1-wbt-dance") + + +def _profile_motion(profile): + """Reload the compiled motion clip from the profile's NPZ payload.""" + import io + + payload = next(iter(profile.payloads.values())) + with np.load(io.BytesIO(payload), allow_pickle=False) as arrays: + return G1WbtMotion(arrays["joint_pos"], arrays["joint_vel"], arrays["reference_quaternion_xyzw"]) + + +def _random_quaternion(rng): + quaternion = rng.normal(size=4) + return (quaternion / np.linalg.norm(quaternion)).astype(np.float32) + + +def _rotation(quaternion): + """Independent Rodrigues construction, insensitive to quaternion sign.""" + vector = np.asarray(quaternion[:3], dtype=np.float64) + scalar = float(quaternion[3]) + x, y, z = vector + skew = np.array([[0, -z, y], [z, 0, -x], [-y, x, 0]]) + norm_squared = np.dot(vector, vector) + scalar**2 + return np.eye(3) + (2 / norm_squared) * (scalar * skew + skew @ skew) + + +def _waist_rotation(yaw, roll, pitch): + cy, sy = np.cos(yaw), np.sin(yaw) + cr, sr = np.cos(roll), np.sin(roll) + cp, sp = np.cos(pitch), np.sin(pitch) + rz = np.array([[cy, -sy, 0], [sy, cy, 0], [0, 0, 1]]) + rx = np.array([[1, 0, 0], [0, cr, -sr], [0, sr, cr]]) + ry = np.array([[cp, 0, sp], [0, 1, 0], [-sp, 0, cp]]) + return rz @ rx @ ry + + +def _state(robot, quaternion, waist): + joints = robot.default_joint_position.copy() + for axis, angle in zip(("yaw", "roll", "pitch"), waist): + joints[robot.joint_names.index(f"waist_{axis}_joint")] = angle + return RobotState( + sample_time_ns=0, + receive_time_ns=0, + joint_position=joints, + joint_velocity=np.zeros(robot.joint_count, np.float32), + base_orientation_xyzw=quaternion, + base_angular_velocity=np.zeros(3, np.float32), + base_linear_acceleration=np.zeros(3, np.float32), + ) + + +@pytest.mark.parametrize("permuted", [False, True]) +@pytest.mark.parametrize( + "waist", + [(0, 0, 0), (0.7, 0, 0), (0, -0.3, 0), (0, 0, 0.4), (0.7, -0.3, 0.4), (-0.8, 0.2, -0.3)], +) +def test_measured_fk_axis_order_base_rotation_and_named_joint_mapping(profile, permuted, waist): + robot = profile.robot + if permuted: + order = np.random.default_rng(13).permutation(robot.joint_count) + robot = replace( + robot, + joint_names=tuple(robot.joint_names[index] for index in order), + default_joint_position=robot.default_joint_position[order], + position_lower=robot.position_lower[order], + position_upper=robot.position_upper[order], + torque_limit=robot.torque_limit[order], + ) + task = G1WbtPolicyProcessor(profile.task, robot, _profile_motion(profile)) + base = _random_quaternion(np.random.default_rng(19)) + state = _state(robot, base, waist) + expected = _rotation(base) @ _waist_rotation(*waist) + np.testing.assert_allclose(_rotation(task.torso_orientation(state)), expected, atol=1e-6, rtol=0) + state.base_orientation_xyzw = -base + np.testing.assert_allclose(_rotation(task.torso_orientation(state)), expected, atol=1e-6, rtol=0) + + +def test_actor_orientation_uses_measured_state(profile): + task = G1WbtPolicyProcessor(profile.task, profile.robot, motion := _profile_motion(profile)) + state = _state(profile.robot, _random_quaternion(np.random.default_rng(37)), (0.8, -0.25, 0.3)) + context = ControlContext(0, 0.0, None, dt_s=0.02) + observation = task.build_observation(state, context) + expected = (_rotation(task.torso_orientation(state)).T @ _rotation(motion.reference_quaternion_xyzw[0]))[ + :2 + ].reshape(-1) + np.testing.assert_allclose(observation[58:64], expected, atol=1e-6, rtol=0) + + +def _set_fk_pose(runtime, backend, orientation, joints): + """Test-only pose injection through simulator APIs, not deployment contracts.""" + robot = runtime.scene.objs.robot + if backend == "mujoco": + model, data = runtime.model, runtime.data + root = model.body(robot.resolved_base_link_name).id + address = model.jnt_qposadr[model.body_jntadr[root]] + data.qpos[address : address + 3] = [0.2, -0.1, 2.0] + data.qpos[address + 3 : address + 7] = orientation[[3, 0, 1, 2]] + for name, position in zip(runtime.robot.spec.joint_names, joints): + data.qpos[model.jnt_qposadr[model.joint(robot.resolve_name(name)).id]] = position + runtime.mj.mj_forward(model, data) + else: + import motrixsim as mtx + + program = runtime.model.compile_write( + { + "position": mtx.write.BodyPosition((robot.resolved_base_link_name,)), + "orientation": mtx.write.BodyRotation((robot.resolved_base_link_name,)), + "joints": mtx.write.BodyJointPosition( + tuple(robot.resolve_name(name) for name in runtime.robot.spec.joint_names) + ), + }, + forward_kinematic=True, + ).allocate(runtime.data) + program["position"][:] = [0.2, -0.1, 2.0] + program["orientation"][:] = orientation + program["joints"][:] = joints + program.execute(runtime.data) + + +@pytest.mark.parametrize("backend,solver_iterations", [("mujoco", 100), ("motrixsim", 3)]) +def test_measured_fk_matches_randomized_simulator_world_orientation(profile, backend, solver_iterations): + pytest.importorskip(backend) + config = SimulationRuntimeConfig( + scene=SceneCfg(objs=_FlatG1Objs(robot=UnitreeG129Dof())), + physics=SimCfg(dt=0.005, solver_iterations=solver_iterations), + sensor_bindings=SensorBindings(base_angular_velocity="base_gyro", base_linear_acceleration="base_accel"), + ) + runtime = create_simulation_runtime(backend, config) + task = G1WbtPolicyProcessor(profile.task, profile.robot, motion := _profile_motion(profile)) + rng = np.random.default_rng(2026) + try: + profile.robot.validate_compatible(runtime.robot.spec) + runtime.open() + for case in range(64): + joints = rng.uniform(profile.robot.position_lower, profile.robot.position_upper).astype(np.float32) + # All three waist angles are independently sampled, not yaw-only probes. + _set_fk_pose(runtime, backend, _random_quaternion(rng), joints) + state = runtime.robot.read_state(0.1) + np.testing.assert_allclose(state.joint_position, joints, atol=1e-6, rtol=0) + # Direct simulator query is a test oracle, not public deployment state. + name = runtime.scene.objs.robot.resolve_name("torso_link") + if backend == "mujoco": + truth = runtime.data.xmat[runtime.model.body(name).id].reshape(3, 3).copy() + else: + import motrixsim as mtx + + query = runtime.model.compile_query({"truth": mtx.query.LinkRotation((name,))}).allocate(runtime.data) + query.execute(runtime.data) + truth = _rotation(query["truth"].reshape(4)) + np.testing.assert_allclose( + _rotation(task.torso_orientation(state)), truth, atol=1e-5, rtol=0, err_msg=f"{backend} case {case}" + ) + # Neither orientation nor position body queries are needed by actor I/O. + observation = task.build_observation(state, ControlContext(0, 0.0, None, dt_s=0.02)) + expected = (truth.T @ _rotation(motion.reference_quaternion_xyzw[0]))[:2].reshape(-1) + np.testing.assert_allclose(observation[58:64], expected, atol=1e-5, rtol=0) + finally: + runtime.close() diff --git a/motrix_deploy_tasks/tests/test_g1_wbt_task.py b/motrix_deploy_tasks/tests/test_g1_wbt_task.py new file mode 100644 index 00000000..bc1ff094 --- /dev/null +++ b/motrix_deploy_tasks/tests/test_g1_wbt_task.py @@ -0,0 +1,505 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Standard WBT deploy lifecycle and self-contained motion contracts.""" + +import numpy as np +import pytest +from pydantic import ValidationError as PydanticValidationError + +from motrix_deploy.contracts import RobotSpec, RobotState +from motrix_deploy.errors import ValidationError +from motrix_deploy.policy import NoOpPolicy +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy_tasks.tasks.g1_wbt import G1WbtDeployTask, G1WbtMotion, G1WbtPolicyProcessor, G1WbtTaskSpec + + +def _motion(reference_quaternions=None): + return G1WbtMotion( + np.asarray([[0.0] * 29, [0.1] * 29], np.float32), + np.asarray([[0.0] * 29, [0.2] * 29], np.float32), + np.asarray(reference_quaternions or [[0, 0, 0, 1]] * 2, np.float32), + ) + + +def _task( + reference_quaternions=None, + *, + controller=False, + steps=None, + automatic=False, + preparation=None, +): + motion = _motion(reference_quaternions) + spec = G1WbtTaskSpec( + action_scale=[0.5] * 29, + kp=[10] * 29, + kd=[1] * 29, + motion_frames=len(motion), + termination_ref_orientation_threshold=0.4, + termination_joint_position_threshold=0.5, + termination_joint_velocity_threshold=10, + **(preparation or {}), + ) + robot = RobotSpec( + "pelvis", + tuple(["waist_yaw_joint", "waist_roll_joint", "waist_pitch_joint", *[f"joint_{i}" for i in range(26)]]), + np.zeros(29, np.float32), + np.full(29, -1, np.float32), + np.ones(29, np.float32), + np.full(29, 10, np.float32), + ) + return ( + G1WbtDeployTask(spec, robot, NoOpPolicy(29), steps=steps, automatic_start=automatic, motion=motion) + if controller + else G1WbtPolicyProcessor(spec, robot, motion) + ) + + +def _state(): + return RobotState( + 0, + 0, + np.zeros(29, np.float32), + np.zeros(29, np.float32), + np.array([0, 0, 0, 1], np.float32), + np.array([1, 2, 3], np.float32), + np.zeros(3, np.float32), + ) + + +# Ramp- and settle-free preparation for lifecycle tests that exercise handover timing. +_instant = {"preparation_ramp_duration_s": 0.0, "preparation_settle_duration_s": 0.0} + + +def test_standard_task_raw_history_servo_limits_frame_and_reset(): + task = _task() + state = _state() + context = ControlContext(1, 0.02, None, dt_s=0.02) + task.validate_command(None) + raw = np.full(29, 9, np.float32) + command = task.process_action(raw) + raw.fill(0) + np.testing.assert_array_equal(command.joint_position, task.robot.position_upper) + np.testing.assert_array_equal(command.kp, task.spec.kp) + observation = task.build_observation(state, context) + assert observation.shape == (154,) + np.testing.assert_allclose(observation[:29], task.motion.joint_pos[1]) + np.testing.assert_array_equal(observation[64:67], state.base_angular_velocity) + np.testing.assert_array_equal(observation[125:], 9) + assert task.check_termination(state) is None + task.reset(state, context) + np.testing.assert_array_equal(task.build_observation(state, context)[125:], 0) + assert G1WbtDeployTask.default_duration_s(task.spec) == 2 * task.spec.period_s + with pytest.raises(ValidationError, match="context.step"): + task.build_observation(state, ControlContext(2, 0.04, None, dt_s=0.02)) + + +def test_orientation_termination_uses_latest_observed_reference_frame(): + task = _task(reference_quaternions=[[0, 0, 0, 1], [1, 0, 0, 0]]) + state = _state() + task.build_observation(state, ControlContext(0, 0, None, dt_s=0.02)) + assert task.check_termination(state) is None + task.build_observation(state, ControlContext(1, 0.02, None, dt_s=0.02)) + assert task.check_termination(state) == "bad_ref_ori" + + +def test_measured_base_and_waist_orientation_and_joint_safety_termination(): + task = _task() + state = _state() + state.base_orientation_xyzw[:] = [1, 0, 0, 0] + assert task.check_termination(state) == "bad_ref_ori" + # Torso FK sees waist pitch, not only the pelvis quaternion. + state.base_orientation_xyzw[:] = [0, 0, 0, 1] + state.joint_position[2] = 1 + assert task.check_termination(state) == "bad_ref_ori" + state.joint_position.fill(0) + state.joint_position[0] = task.robot.position_upper[0] + 1 + assert task.check_termination(state) == "bad_dof_pos" + state.joint_position.fill(0) + state.joint_velocity[0] = task.spec.termination_joint_velocity_threshold + 1 + assert task.check_termination(state) == "bad_dof_vel" + + +def test_spec_roundtrip_and_motion_shape_validation(): + task = _task() + restored = G1WbtTaskSpec.model_validate_json(task.spec.model_dump_json()) + assert restored == task.spec + with pytest.raises(ValidationError, match="motion.joint_vel"): + G1WbtMotion(task.motion.joint_pos, task.motion.joint_vel[:1], task.motion.reference_quaternion_xyzw) + state = _state() + # Both actor input and safety checks use the same hardware-observable state. + assert task.build_observation(state, ControlContext(0, 0, None, dt_s=0.02)).shape == (154,) + assert task.check_termination(state) is None + + +def test_motion_validates_frame_counts_and_unit_quaternions(): + motion = _motion() + with pytest.raises(ValidationError, match="motion.joint_vel"): + G1WbtMotion(motion.joint_pos, motion.joint_vel[:1], motion.reference_quaternion_xyzw) + with pytest.raises(ValidationError, match="motion.reference_quaternion_xyzw"): + G1WbtMotion(motion.joint_pos, motion.joint_vel, motion.reference_quaternion_xyzw[:1]) + tilted = motion.reference_quaternion_xyzw.copy() + tilted[0] = [0, 0, 0, 2] + with pytest.raises(ValidationError, match="unit xyzw quaternions"): + G1WbtMotion(motion.joint_pos, motion.joint_vel, tilted) + + +def test_preparation_commands_joint_posture_not_root_state_or_motion_velocity(): + task = _task(controller=True, preparation={"preparation_ramp_duration_s": 0.0}) + state = _state() + ready = task.ready + ready.reset(state) + transition = ready.step(state, ControlContext(0, 0, None, dt_s=0.02)) + command = transition.command + np.testing.assert_array_equal(command.joint_position, task.motion.joint_pos[0]) + np.testing.assert_array_equal(command.kp, task.spec.kp) + np.testing.assert_array_equal(command.kd, task.spec.kd) + np.testing.assert_array_equal(command.joint_velocity, 0) + np.testing.assert_array_equal(command.feedforward_torque, 0) + assert command.joint_position.dtype == np.float32 + + +def test_preparation_ramps_from_first_step_measurement_then_requires_continuous_readiness(): + task = _task( + controller=True, + preparation={ + "preparation_ramp_duration_s": 0.4, + "preparation_settle_duration_s": 0.2, + "preparation_timeout_s": 2.0, + }, + ) + state = _state() + state.joint_position.fill(0.8) + controller = task.ready + state.joint_position.fill(0.6) + controller.reset(state) + first = controller.step(state, ControlContext(0, 0, None, dt_s=0.1)) + np.testing.assert_allclose(first.command.joint_position, 0.45) + np.testing.assert_array_equal(first.command.kp, task.spec.kp) + np.testing.assert_array_equal(first.command.kd, task.spec.kd) + state.joint_position.fill(0) + assert not controller.step( + state, ControlContext(0, 0.3, None, dt_s=0.02) + ).complete # Ready joints do not bypass the ramp. + assert not controller.step(state, ControlContext(0, 0.4, None, dt_s=0.02)).complete + state.joint_velocity.fill(2) + assert not controller.step(state, ControlContext(0, 0.5, None, dt_s=0.02)).complete + state.joint_velocity.fill(0) + assert not controller.step(state, ControlContext(0, 0.6, None, dt_s=0.02)).complete + assert not controller.step(state, ControlContext(0, 0.7, None, dt_s=0.02)).complete + ready = controller.step(state, ControlContext(0, 0.8, None, dt_s=0.02)) + assert ready.complete + assert ready.command is None + assert ready.error is None + + +def test_preparation_targets_standing_pose_and_clips_to_physical_joint_limits(): + base = _task(controller=True, preparation=_instant) + motion = G1WbtMotion( # Motion frame 0 must not drive preparation. + np.full((2, 29), 2.0, np.float32), + np.zeros((2, 29), np.float32), + np.tile([0, 0, 0, 1], (2, 1)).astype(np.float32), + ) + task = G1WbtDeployTask(base.spec, base.robot, NoOpPolicy(29), automatic_start=False, motion=motion) + state = _state() + controller = task.ready + controller.reset(state) + standing = controller.step(state, ControlContext(0, 0, None, dt_s=0.02)) + assert standing.complete # Standing readiness ignores the motion frame. + assert standing.command is None # Completion carries no command; the task holds. + hold = controller.hold_command() + np.testing.assert_array_equal(hold.joint_position, task.robot.default_joint_position) + np.testing.assert_array_equal(hold.kp, task.spec.kp) + np.testing.assert_array_equal(hold.kd, task.spec.kd) + np.testing.assert_array_equal(hold.joint_velocity, 0) + np.testing.assert_array_equal(hold.feedforward_torque, 0) + clipped = G1WbtDeployTask( + task.spec.model_copy(update={"preparation_joint_position": [2] * 29}), + task.robot, + NoOpPolicy(29), + motion=motion, + ) + np.testing.assert_array_equal(clipped.ready.hold_command().joint_position, task.robot.position_upper) + + +def test_preparation_rejects_unsafe_tilt_and_times_out_without_readiness(): + task = _task(controller=True) + state = _state() + controller = task.ready + controller.reset(state) + state.base_orientation_xyzw[:] = [1, 0, 0, 0] + unsafe = controller.step(state, ControlContext(0, 0, None, dt_s=0.02)) + assert "unsafe base tilt" in unsafe.error + assert unsafe.command is None + state.base_orientation_xyzw[:] = [0, 0, 0, 1] + state.joint_velocity.fill(2) + timeout = controller.step(state, ControlContext(0, 5, None, dt_s=0.02)) + assert "timed out" in timeout.error + assert not timeout.complete + assert timeout.command is None + + +@pytest.mark.parametrize( + "field,value", + [ + ("preparation_timeout_s", 0), + ("preparation_ramp_duration_s", -1), + ("preparation_settle_duration_s", -0.5), + ("preparation_timeout_s", float("nan")), + ], +) +def test_spec_validates_preparation_timing(field, value): + data = _task().spec.model_dump() + data[field] = value + with pytest.raises(PydanticValidationError, match=field): + G1WbtTaskSpec.model_validate(data) + + +def test_normal_motion_stopping_is_finite_measured_damping_without_position_hold(): + task = _task(controller=True) + state = _state() + controller = task.damping + state.joint_position.fill(0.4) + state.joint_velocity.fill(0.7) + state.base_orientation_xyzw[:] = [1, 0, 0, 0] # Damping does not demand a ready posture. + command = controller.step(state, ControlContext(0, 0, None, dt_s=0.02)).command + np.testing.assert_array_equal(command.joint_position, state.joint_position) + np.testing.assert_array_equal(command.kp, 0) + np.testing.assert_array_equal(command.kd, task.spec.kd) + np.testing.assert_array_equal(command.joint_velocity, 0) + np.testing.assert_array_equal(command.feedforward_torque, 0) + state.joint_position.fill(0.2) + np.testing.assert_array_equal( + controller.step(state, ControlContext(0, 0.2, None, dt_s=0.02)).command.joint_position, state.joint_position + ) + state.joint_position[0] = task.robot.position_upper[0] + 0.1 + state.joint_position[1] = task.robot.position_lower[1] - 0.1 + clipped = controller.step(state, ControlContext(0, 0.25, None, dt_s=0.02)).command + assert clipped.joint_position[0] == task.robot.position_upper[0] + assert clipped.joint_position[1] == task.robot.position_lower[1] + torque = ( + clipped.kp * (clipped.joint_position - state.joint_position) + + clipped.kd * (clipped.joint_velocity - state.joint_velocity) + + clipped.feedforward_torque + ) + np.testing.assert_array_equal(torque, -clipped.kd * state.joint_velocity) + complete = controller.step(state, ControlContext(0, 0.3, None, dt_s=0.02)) + assert complete.complete + assert complete.command is None + assert complete.error is None + + +def test_composite_handover_hold_playback_clocks_and_finite_damping(): + task = _task( + controller=True, preparation={"preparation_ramp_duration_s": 0.0, "preparation_settle_duration_s": 0.02} + ) + controller = task + state = _state() + seen = [] + observations = [] + observe = controller.policy_io.build_observation + + def observation(measurement, context): + record = observe(measurement, context) + seen.append((measurement, context.step, context.elapsed_time_s)) + observations.append(record) + return record + + controller.policy_io.build_observation = observation + controller.reset(state) + assert controller.step(state, ControlContext(0, 3.0, None, dt_s=0.02)).command is not None + assert controller.policy_controller.completed_steps == 0 + controller.request_policy_start() + # Readiness completion and the first held model decision use the same sample and tick. + assert controller.step(state, ControlContext(1, 3.02, None, dt_s=0.02)).command is not None + assert controller.phase == "policy_hold" + assert seen == [(state, 0, 0.0)] + np.testing.assert_allclose(observations[0][:29], 0.0) # Hold freezes reference frame 0. + assert controller.transitions[0].complete + assert controller.transitions[0].name == "prepare" + controller.request_motion_start() + assert controller.step(state, ControlContext(2, 3.04, None, dt_s=0.02)).command is not None + assert controller.phase == "playback" + # Inference history is preserved: the model clock continues from the hold origin. + assert seen[-1][:2] == (state, 1) + assert seen[-1][2] == pytest.approx(0.02) + assert controller.playback_steps == 1 + assert controller.policy_controller.completed_steps == 2 + assert controller.step(state, ControlContext(3, 3.06, None, dt_s=0.02)).command is not None + np.testing.assert_allclose(observations[2][:29], 0.1) # Only playback advances the frame. + assert controller.playback_steps == 2 + assert controller.policy_controller.completed_steps == 3 + # Budget completion starts damping immediately, not an empty interval. + damping = controller.step(state, ControlContext(4, 3.08, None, dt_s=0.02)) + np.testing.assert_array_equal(damping.command.kp, 0) + assert controller.policy_controller.completed_steps == 3 + assert controller.playback_steps == 2 + assert controller.transitions[-1].name == "damping" + assert controller.transitions[-1].time_s == 0.02 + for step in range(5, 19): + assert not controller.step(state, ControlContext(step, 3.0 + step * 0.02, None, dt_s=0.02)).complete + assert controller.step(state, ControlContext(19, 3.38, None, dt_s=0.02)).complete + assert controller.transitions[-1].complete + + +def test_composite_stop_request_during_ready_hold_enters_damping(): + controller = _task(controller=True, steps=2, preparation=_instant) + state = _state() + controller.reset(state) + assert controller.step(state, ControlContext(0, 0.0, None, dt_s=0.02)).command is not None + assert controller.preparation_ready + assert controller.phase == "prepare" + controller.request_stop() + decision = controller.step(state, ControlContext(1, 0.02, None, dt_s=0.02)) + np.testing.assert_array_equal(decision.command.kp, 0) + assert controller.phase == "damping" + assert controller.policy_controller.completed_steps == 0 # Stopped before takeover. + assert not controller.transitions[0].complete + assert controller.transitions[-1].name == "damping" + + +def test_early_latched_requests_wait_for_measured_readiness_and_hold_once(): + controller = _task( + controller=True, + preparation={"preparation_ramp_duration_s": 0.1, "preparation_settle_duration_s": 0.1}, + ) + state = _state() + controller.reset(state) + controller.request_policy_start() + controller.request_motion_start() + for step in range(0, 10): + decision = controller.step(state, ControlContext(step, step * 0.02, None, dt_s=0.02)) + assert controller.phase == "prepare" # Latched intent cannot bypass ramp or settle. + assert decision.command is not None + assert controller.policy_controller.completed_steps == 0 + assert not controller.preparation_ready # Settled readiness is not yet complete. + # Settled readiness hands over on the same tick, but playback waits one hold interval. + controller.step(state, ControlContext(10, 0.2, None, dt_s=0.02)) + assert controller.phase == "policy_hold" + assert controller.policy_controller.completed_steps == 1 + assert controller.playback_steps == 0 + controller.step(state, ControlContext(11, 0.22, None, dt_s=0.02)) + assert controller.phase == "playback" + assert controller.playback_steps == 1 + + +def test_task_preparation_times_out_before_first_readiness(): + controller = _task(controller=True, steps=2, preparation=_instant) + state = _state() + state.joint_velocity.fill(2) # Continuous readiness is never reached. + controller.reset(state) + controller.request_policy_start() # Latched intent does not bypass the gate. + assert controller.step(state, ControlContext(0, 0.0, None, dt_s=0.02)).command is not None + failed = controller.step(state, ControlContext(250, 5.0, None, dt_s=0.02)) # Stage-local 5 s timeout. + assert "timed out" in failed.error + assert failed.command is None + assert controller.phase == "prepare" + assert controller.policy_controller.completed_steps == 0 + + +def test_composite_checks_last_model_interval_before_damping(): + from motrix_deploy.errors import ControlFailure + + task = _task(controller=True, preparation=_instant) + controller = G1WbtDeployTask( + task.spec, task.robot, NoOpPolicy(29), steps=1, automatic_start=False, motion=task.motion + ) + state = _state() + controller.reset(state) + controller.request_policy_start() + controller.request_motion_start() + assert controller.step(state, ControlContext(0, 0.0, None, dt_s=0.02)).command is not None + assert controller.phase == "policy_hold" # At least one hold interval first. + assert controller.step(state, ControlContext(1, 0.02, None, dt_s=0.02)).command is not None + assert controller.phase == "playback" + assert controller.playback_steps == 1 + state.base_orientation_xyzw[:] = [1, 0, 0, 0] + # Termination is checked on the measured state before budget completion can finish. + with pytest.raises(ControlFailure, match="bad_ref_ori"): + controller.step(state, ControlContext(2, 0.04, None, dt_s=0.02)) + assert len(controller.transitions) == 3 # Faults do not enter normal damping. + assert controller.transitions[-1].name == "playback" + + +class _FakeKeyboard: + """Deliver per-frame press edges exactly as an event-framed device reports them.""" + + def __init__(self): + self._pending = {} + self._active = {} + + def press(self, *keys) -> None: + self._pending = dict.fromkeys(keys, True) + + def poll(self) -> None: + self._active, self._pending = self._pending, {} + + def is_key_down(self, key: str) -> bool: + return self._active.get(key, False) + + +def test_attached_keyboard_press_edges_drive_phase_requests(): + task = _task(controller=True, steps=2, preparation=_instant) + keyboard = _FakeKeyboard() + task.attach_keyboard(keyboard) + state = _state() + task.reset(state) + assert task.step(state, ControlContext(0, 0.0, None, dt_s=0.02)).command is not None + assert task.preparation_ready and task.phase == "prepare" # Ready, waiting for input. + keyboard.press("p") + decision = task.step(state, ControlContext(1, 0.02, None, dt_s=0.02)) + assert decision.command is not None and task.phase == "policy_hold" + # Default zero hold target: the next held tick enters playback by itself. + task.step(state, ControlContext(2, 0.04, None, dt_s=0.02)) + assert task.phase == "playback" + + +def test_attached_keyboard_stop_request_enters_damping(): + task = _task(controller=True, preparation=_instant) + keyboard = _FakeKeyboard() + task.attach_keyboard(keyboard) + state = _state() + task.reset(state) + task.step(state, ControlContext(0, 0.0, None, dt_s=0.02)) + task.step(state, ControlContext(1, 0.02, None, dt_s=0.02)) + keyboard.press("o") + decision = task.step(state, ControlContext(2, 0.04, None, dt_s=0.02)) + np.testing.assert_array_equal(decision.command.kp, 0) + assert task.phase == "damping" + assert task.policy_controller.completed_steps == 0 + + +def test_automatic_start_gates_on_readiness_and_hold_ticks(): + task = _task(controller=True, steps=2, preparation=_instant) + task.enable_automatic_start(hold_ticks=2) + state = _state() + task.reset(state) + task.step(state, ControlContext(0, 0.0, None, dt_s=0.02)) + assert task.phase == "prepare" and task.preparation_ready + task.step(state, ControlContext(1, 0.02, None, dt_s=0.02)) + assert task.phase == "policy_hold" # Takeover happens on the tick after readiness. + task.step(state, ControlContext(2, 0.04, None, dt_s=0.02)) + assert task.phase == "policy_hold" # First held tick below the target. + task.step(state, ControlContext(3, 0.06, None, dt_s=0.02)) + assert task.phase == "playback" + + +def test_operator_input_rejects_invalid_configuration(): + task = _task(controller=True) + with pytest.raises(ValidationError, match="operator.keyboard"): + task.attach_keyboard(None) + + +def test_default_automatic_start_progresses_after_measured_readiness(): + task = _task(controller=True, steps=2, automatic=True, preparation=_instant) + state = _state() + task.reset(state) + assert task.step(state, ControlContext(0, 0.0, None, dt_s=0.02)).command is not None + assert task.preparation_ready and task.phase == "prepare" + # Takeover happens on the tick after readiness without any request call. + task.step(state, ControlContext(1, 0.02, None, dt_s=0.02)) + assert task.phase == "policy_hold" + # Default zero hold target: the next held tick enters playback by itself. + task.step(state, ControlContext(2, 0.04, None, dt_s=0.02)) + assert task.phase == "playback" diff --git a/motrix_deploy_tasks/tests/test_go2_walk.py b/motrix_deploy_tasks/tests/test_go2_walk.py index 809a4b3a..03da6574 100644 --- a/motrix_deploy_tasks/tests/test_go2_walk.py +++ b/motrix_deploy_tasks/tests/test_go2_walk.py @@ -11,9 +11,11 @@ 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.tasks.go2_walk import Go2WalkDeployTaskV1, Go2WalkTaskSpec +from motrix_deploy.errors import ControlFailure, ValidationError +from motrix_deploy.policy import NoOpPolicy +from motrix_deploy.runtime import ControlContext +from motrix_deploy.runtime.control import PolicyController +from motrix_deploy_tasks.tasks.go2_walk import Go2WalkDeployTask, Go2WalkPolicyProcessor, Go2WalkTaskSpec from motrix_env_core.input import PlanarVelocityCommand @@ -21,11 +23,12 @@ 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( +def _context(step: int, elapsed_time_s: float, velocity: list[float] | np.ndarray) -> ControlContext: + return ControlContext( step=step, elapsed_time_s=elapsed_time_s, command=PlanarVelocityCommand(np.asarray(velocity, dtype=np.float32)[None, :]), + dt_s=0.02, ) @@ -62,7 +65,6 @@ def _task_spec() -> Go2WalkTaskSpec: 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, @@ -70,6 +72,22 @@ def _task_spec() -> Go2WalkTaskSpec: ) +def test_go2_missing_command_is_rejected_before_policy_and_action(monkeypatch): + robot = _robot() + task = Go2WalkPolicyProcessor(_task_spec(), robot) + policy = NoOpPolicy(robot.joint_count) + controller = PolicyController(task, policy, steps=1) + calls = [] + monkeypatch.setattr(policy, "infer", lambda observation: calls.append("infer")) + monkeypatch.setattr(task, "process_action", lambda action: calls.append("action")) + state = _state(robot) + controller.reset(state) + with pytest.raises(ControlFailure) as failure: + controller.step(state, ControlContext(0, 0.0, None, dt_s=0.02)) + assert failure.value.reason == "input_error" + assert not calls + + @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() @@ -77,7 +95,7 @@ def test_fall_orientation_uses_artifact_threshold(up_z: float) -> None: 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) + task = Go2WalkPolicyProcessor(spec, robot) reason = task.check_termination(state) assert (reason == "fall_orientation") == (up_z <= spec.termination_min_up_z) @@ -85,29 +103,11 @@ def test_fall_orientation_uses_artifact_threshold(up_z: float) -> None: 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) + task = Go2WalkPolicyProcessor(_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"]) +@pytest.mark.parametrize("key", ["termination_min_up_z", "standing_threshold"]) def test_missing_required_artifact_field_is_rejected_during_decoding(key: str) -> None: wire = _task_spec().to_dict() del wire["config"][key] @@ -125,7 +125,7 @@ def test_typed_task_spec_roundtrips_through_common_codec() -> None: @pytest.mark.parametrize( ("key", "value"), - [("termination_min_up_z", 2.0), ("termination_min_up_z", True), ("termination_min_base_height", float("nan"))], + [("termination_min_up_z", 2.0), ("termination_min_up_z", True)], ) def test_termination_config_is_validated(key: str, value: object) -> None: with pytest.raises(PydanticValidationError, match=key): @@ -194,7 +194,7 @@ def test_invalid_wire_types_are_rejected_by_common_codec(key: str, value: object TaskSpec.from_dict(wire) -@pytest.mark.parametrize("key", ["gait_frequency_hz", "standing_threshold", "termination_min_base_height"]) +@pytest.mark.parametrize("key", ["gait_frequency_hz", "standing_threshold"]) def test_programmatic_numeric_fields_reject_booleans(key: str) -> None: with pytest.raises(PydanticValidationError, match=key): _replace_spec(_task_spec(), **{key: True}) @@ -283,7 +283,7 @@ def test_runtime_checks_robot_joint_count() -> None: torque_limit=robot.torque_limit[:-1], ) with pytest.raises(ValidationError, match="task.config.action_scale"): - Go2WalkDeployTaskV1(_task_spec(), short_robot) + Go2WalkPolicyProcessor(_task_spec(), short_robot) joint_count = short_robot.joint_count spec = _replace_spec( @@ -294,7 +294,7 @@ def test_runtime_checks_robot_joint_count() -> None: action_lower=[-1.0] * joint_count, action_upper=[1.0] * joint_count, ) - task = Go2WalkDeployTaskV1(spec, short_robot) + task = Go2WalkPolicyProcessor(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,) @@ -305,7 +305,7 @@ def test_runtime_checks_robot_joint_count() -> None: def test_go2_observation_has_exact_49_element_contract() -> None: robot = _robot() state = _state(robot) - task = Go2WalkDeployTaskV1(_task_spec(), robot) + task = Go2WalkPolicyProcessor(_task_spec(), robot) context = _context(0, 0.0, [0.5, 0.0, -0.2]) task.validate_command(context.command) task.reset(state, context) @@ -324,7 +324,7 @@ def test_go2_observation_has_exact_49_element_contract() -> None: def test_previous_action_and_trot_phase_advance_after_first_tick() -> None: robot = _robot() state = _state(robot) - task = Go2WalkDeployTaskV1(_task_spec(), robot) + task = Go2WalkPolicyProcessor(_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) @@ -340,7 +340,7 @@ def test_previous_action_and_trot_phase_advance_after_first_tick() -> None: def test_standing_command_freezes_deployment_gait_phase() -> None: robot = _robot() - task = Go2WalkDeployTaskV1(_task_spec(), robot) + task = Go2WalkPolicyProcessor(_task_spec(), robot) context = _context(10, 0.2, np.zeros(3, dtype=np.float32)) task.validate_command(context.command) @@ -364,7 +364,7 @@ def test_action_processing_applies_clip_scale_and_position_limits() -> None: action_lower=[-2.0] * 12, action_upper=[0.5] * 12, ) - task = Go2WalkDeployTaskV1(spec, robot) + task = Go2WalkPolicyProcessor(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) @@ -382,15 +382,35 @@ def test_action_processing_applies_clip_scale_and_position_limits() -> None: def test_go2_task_rejects_non_singleton_command_batch() -> None: - task = Go2WalkDeployTaskV1(_task_spec(), _robot()) + task = Go2WalkPolicyProcessor(_task_spec(), _robot()) with pytest.raises(ValidationError, match="command.batch_size"): task.validate_command(PlanarVelocityCommand(np.zeros((2, 3), dtype=np.float32))) def test_go2_task_scales_command_range_for_deployment_input_mapping() -> None: - task = Go2WalkDeployTaskV1(_task_spec(), _robot()) + task = Go2WalkPolicyProcessor(_task_spec(), _robot()) np.testing.assert_array_equal(task.command_lower, [-0.5, -0.5, -0.5]) np.testing.assert_array_equal(task.command_upper, [0.5, 0.5, 0.5]) task.validate_command(PlanarVelocityCommand(np.array([[0.75, 0.0, 0.0]], dtype=np.float32))) + + +def test_go2_root_task_executes_policy_budget_and_normal_stop(): + robot = _robot() + task = Go2WalkDeployTask(_task_spec(), robot, NoOpPolicy(robot.joint_count), steps=2) + state = _state(robot) + task.reset(state) + for step in range(2): + decision = task.step(state, _context(step, step * 0.02, np.zeros(3, dtype=np.float32))) + assert decision.command is not None and decision.policy_tick + completed = task.step(state, _context(2, 0.04, np.zeros(3, dtype=np.float32))) + assert completed.complete and completed.success + assert task.policy_controller.completed_steps == 2 + + task.reset(state) + assert task.step(state, _context(0, 0.0, np.zeros(3, dtype=np.float32))).policy_tick + task.request_stop() + stopped = task.step(state, _context(1, 0.02, np.zeros(3, dtype=np.float32))) + assert stopped.complete and not stopped.success + assert task.policy_controller.completed_steps == 1 diff --git a/motrix_deploy_tasks/tests/test_training_parity.py b/motrix_deploy_tasks/tests/test_training_parity.py index 985bf19e..8a7431e4 100644 --- a/motrix_deploy_tasks/tests/test_training_parity.py +++ b/motrix_deploy_tasks/tests/test_training_parity.py @@ -1,16 +1,18 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Golden parity between Go2 training and deployment observation/action semantics.""" +"""Training/deployment observation, action and embedded motion parity.""" + +import io import numpy as np import pytest import motrix_deploy_tasks 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_deploy.runtime import ControlContext +from motrix_deploy_tasks.tasks.g1_wbt import G1WbtTaskSpec +from motrix_deploy_tasks.tasks.go2_walk import Go2WalkPolicyProcessor, Go2WalkTaskSpec from motrix_env_core import registry from motrix_env_core.input import PlanarVelocityCommand from motrix_envs.deploy import build_deployment_profile @@ -25,12 +27,8 @@ def test_go2_training_and_deployment_task_golden_probe(env_name: str) -> None: env_state = env.init_state() probe = _make_env_probe(env) robot_state = _robot_state_from_env(env, env_state, probe) - context = PolicyContext( - step=0, - elapsed_time_s=0.0, - command=PlanarVelocityCommand(env._commands), - ) - task = create_task(profile.task, profile.robot) + context = ControlContext(step=0, elapsed_time_s=0.0, command=PlanarVelocityCommand(env._commands), dt_s=0.02) + task = Go2WalkPolicyProcessor(profile.task, profile.robot) assert isinstance(profile.task, Go2WalkTaskSpec) command_scale = np.asarray(profile.task.command_scale, dtype=np.float32) assert command_scale.shape == (3,) @@ -59,10 +57,8 @@ def test_go2_training_and_deployment_task_golden_probe(env_name: str) -> None: stepped = env.step(raw_action[None, :]) next_state = _robot_state_from_env(env, stepped, probe) - next_context = PolicyContext( - step=1, - elapsed_time_s=profile.control.period_s, - command=PlanarVelocityCommand(env._commands), + next_context = ControlContext( + step=1, elapsed_time_s=profile.control.period_s, command=PlanarVelocityCommand(env._commands), dt_s=0.02 ) task.validate_command(next_context.command) np.testing.assert_allclose( @@ -73,6 +69,40 @@ def test_go2_training_and_deployment_task_golden_probe(env_name: str) -> None: ) +def test_g1_profile_uses_config_not_environment_and_embeds_named_motion(monkeypatch): + pytest.importorskip("motrix_envs") + from motrix_envs.deploy.g1_wbt import build_g1_wbt_profile + from motrix_envs.motion.library import expand_motion_paths + from motrix_envs.motion.loader import MotrixMotion + + def no_environment(*args, **kwargs): + pytest.fail("profile compilation must not construct a training environment") + + monkeypatch.setattr(registry, "make", no_environment) + profile = build_g1_wbt_profile("g1-wbt-dance") + cfg = registry.make_env_config("g1-wbt-dance") + motion = MotrixMotion(expand_motion_paths(cfg.commands.motion.motion_files)[0]) + restored = G1WbtTaskSpec.model_validate_json(profile.task.model_dump_json()) + indices = motion.joint_indices(list(profile.robot.joint_names)) + torso = motion.body_index(cfg.commands.motion.reference_body_name) + payload = np.load(io.BytesIO(next(iter(profile.payloads.values()))), allow_pickle=False) + np.testing.assert_array_equal(payload["joint_pos"], motion.joint_pos[:, indices]) + np.testing.assert_array_equal(payload["joint_vel"], motion.joint_vel[:, indices]) + np.testing.assert_array_equal(payload["reference_quaternion_xyzw"], motion.body_quat_w[:, torso]) + assert restored.motion_frames == len(motion.joint_pos) + assert restored.termination_ref_orientation_threshold == cfg.terminations.bad_ref_ori.threshold + assert restored.termination_joint_position_threshold == cfg.terminations.bad_dof_pos.threshold + assert restored.termination_joint_velocity_threshold == cfg.terminations.bad_dof_vel.threshold + kp = np.asarray(restored.kp) + expected_scale = np.zeros_like(kp) + np.divide(profile.robot.torque_limit, kp, out=expected_scale, where=kp != 0) + expected_scale *= cfg.actions.joint_position.action_scale + np.testing.assert_allclose(restored.action_scale, expected_scale) + assert profile.observation_size == 154 # Actor interface, not task tuning. + assert profile.action_size == profile.robot.joint_count == 29 + assert profile.control.period_s == restored.period_s + + def _make_env_probe(env): """Side compiler reading the imu site pose and live actuator controls.""" diff --git a/motrix_envs/src/motrix_envs/deploy/__init__.py b/motrix_envs/src/motrix_envs/deploy/__init__.py index 4f903eb7..9816b89b 100644 --- a/motrix_envs/src/motrix_envs/deploy/__init__.py +++ b/motrix_envs/src/motrix_envs/deploy/__init__.py @@ -3,7 +3,9 @@ """Deployment profile compilers for built-in MotrixLab environments.""" +# g1_wbt is imported for registration, not as a second public API facade. from motrix_deploy.profile import build_deployment_profile, registered_profile_compilers +from motrix_envs.deploy import g1_wbt as _g1_wbt from motrix_envs.deploy.go2_walk import build_go2_walk_profile __all__ = ["build_deployment_profile", "build_go2_walk_profile", "registered_profile_compilers"] diff --git a/motrix_envs/src/motrix_envs/deploy/g1_wbt.py b/motrix_envs/src/motrix_envs/deploy/g1_wbt.py new file mode 100644 index 00000000..bcacbed1 --- /dev/null +++ b/motrix_envs/src/motrix_envs/deploy/g1_wbt.py @@ -0,0 +1,88 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Compile G1 WBT config and one motion clip without constructing an environment.""" + +import io as io_module + +import numpy as np + +from motrix_deploy.artifact.schema import ControlSpec +from motrix_deploy.profile import DeploymentProfile, register_profile_compiler +from motrix_deploy_tasks.tasks.g1_wbt import G1WbtMotion, G1WbtTaskSpec +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.wbt.g1.common import G1WbtEnvCfg +from motrix_envs.motion.library import expand_motion_paths +from motrix_envs.motion.loader import MotrixMotion + + +@register_profile_compiler("g1-wbt-dance") +def build_g1_wbt_profile(env_name: str) -> DeploymentProfile: + """Embed actor reference channels and hardware-observable safety thresholds.""" + cfg = registry.make_env_config(env_name) + if not isinstance(cfg, G1WbtEnvCfg): + raise TypeError(f"Expected G1WbtEnvCfg, got {type(cfg).__name__}") + motion_cfg = cfg.commands.motion + paths = expand_motion_paths(motion_cfg.motion_files) + if len(paths) != 1: + raise ValueError("G1 WBT deployment v1 requires exactly one motion clip") + motion = MotrixMotion(paths[0]) + if not np.isclose(motion.fps * cfg.ctrl_dt, 1): + raise ValueError("Motion fps must match the deployment control period") + robot_cfg = cfg.scene.objs.robot + model = build_robot_model(robot_cfg) + robot = build_robot_spec(robot_cfg, key_pose_name="default", model=model) + kp, kd = read_position_servo_gains(model, robot.joint_names) + action = cfg.actions.joint_position + if not action.action_scales_by_effort_limit_over_p_gain: + raise ValueError("G1 WBT deployment requires effort/kp action scaling") + scale = np.zeros(robot.joint_count, dtype=np.float32) + np.divide(robot.torque_limit, kp, out=scale, where=kp != 0) + scale *= action.action_scale + joint_indices = motion.joint_indices(list(robot.joint_names)) + reference_index = motion.body_index(motion_cfg.reference_body_name) + term = cfg.terminations + # Standing-hold servo gains for physical preparation, in robot.joint_names order + # (12 legs, 3 waist, 14 arms). Training gains are dynamic whole-body tracking + # gains (legs 28-99) and cannot hold a static standing pose; these firmer gains + # are empirically validated to reach preparation readiness with unchanged safety + # gates (tilt/error/velocity/timeout) and complete the full deployment sequence. + preparation_kp = ([350.0, 200.0, 200.0, 300.0, 300.0, 150.0] * 2) + [200.0] * 3 + [40.0] * 14 + preparation_kd = ([5.0, 5.0, 5.0, 10.0, 5.0, 5.0] * 2) + [5.0] * 3 + [3.0] * 14 + joint_pos = motion.joint_pos[:, joint_indices] + joint_vel = motion.joint_vel[:, joint_indices] + reference_quaternion = motion.body_quat_w[:, reference_index] + clip = G1WbtMotion(joint_pos, joint_vel, reference_quaternion) + # The validated motion clip travels as an artifact NPZ payload; the manifest + # spec keeps only the payload path and the frame count. The task reloads and + # revalidates the clip at construction. + buffer = io_module.BytesIO() + np.savez( + buffer, + joint_pos=clip.joint_pos, + joint_vel=clip.joint_vel, + reference_quaternion_xyzw=clip.reference_quaternion_xyzw, + ) + motion_payload_path = "payloads/motion.npz" + task = G1WbtTaskSpec( + period_s=cfg.ctrl_dt, + action_scale=scale.tolist(), + kp=kp.tolist(), + kd=kd.tolist(), + preparation_kp=preparation_kp, + preparation_kd=preparation_kd, + motion_frames=len(clip), + reference_body_name=motion_cfg.reference_body_name, + termination_ref_orientation_threshold=term.bad_ref_ori.threshold, + termination_joint_position_threshold=term.bad_dof_pos.threshold, + termination_joint_velocity_threshold=term.bad_dof_vel.threshold, + ) + return DeploymentProfile( + robot=robot, + task=task, + control=ControlSpec(period_s=cfg.ctrl_dt, state_timeout_s=0.1), + observation_size=154, + action_size=robot.joint_count, + payloads={motion_payload_path: buffer.getvalue()}, + ) diff --git a/motrix_envs/src/motrix_envs/deploy/go2_walk.py b/motrix_envs/src/motrix_envs/deploy/go2_walk.py index b8b8f0b8..8110273f 100644 --- a/motrix_envs/src/motrix_envs/deploy/go2_walk.py +++ b/motrix_envs/src/motrix_envs/deploy/go2_walk.py @@ -63,7 +63,6 @@ def build_go2_walk_profile(env_name: str) -> DeploymentProfile: standing_threshold=env_cfg.commands.velocity.standing_threshold, # Match training's orientation-only fall termination. termination_min_up_z=env_cfg.termination_min_up_z, - termination_min_base_height=None, kp=kp.tolist(), kd=kd.tolist(), action_lower=action_lower.tolist(), diff --git a/motrix_rl/src/motrix_rl/deploy/service.py b/motrix_rl/src/motrix_rl/deploy/service.py index c1609026..b92e8e56 100644 --- a/motrix_rl/src/motrix_rl/deploy/service.py +++ b/motrix_rl/src/motrix_rl/deploy/service.py @@ -13,6 +13,7 @@ sha256_bytes, write_artifact, ) +from motrix_deploy.artifact.schema import PayloadSpec from motrix_deploy.contracts import TensorSpec from motrix_deploy.profile import DeploymentProfile from motrix_rl import checkpoints @@ -80,8 +81,11 @@ def export_deploy_run( robot=profile.robot, task=profile.task, control=profile.control, + payloads=tuple( + PayloadSpec(path=path, sha256=sha256_bytes(data)) for path, data in sorted(profile.payloads.items()) + ), ) - artifact = write_artifact(output, manifest, {"policy/model.onnx": policy_bytes}) + artifact = write_artifact(output, manifest, {"policy/model.onnx": policy_bytes, **profile.payloads}) return DeploymentExportResult( artifact=artifact, validation_samples=exported.report.parity.samples, diff --git a/motrix_rl/src/motrix_rl/deploy/source_rollout.py b/motrix_rl/src/motrix_rl/deploy/source_rollout.py index 1257b87a..b360eafe 100644 --- a/motrix_rl/src/motrix_rl/deploy/source_rollout.py +++ b/motrix_rl/src/motrix_rl/deploy/source_rollout.py @@ -9,7 +9,7 @@ import numpy as np from motrix_deploy.artifact import read_artifact -from motrix_deploy.policy import OnnxPolicyRuntime +from motrix_deploy.policy import OnnxPolicy from motrix_env_core import registry from motrix_rl.deploy.api import SourceRolloutResult @@ -29,7 +29,7 @@ def validate_motrixsim_source_rollout( if command.shape != (3,) or not np.isfinite(command).all(): raise ValueError(f"command must contain three finite values, got {command}") artifact = read_artifact(artifact_path) - policy = OnnxPolicyRuntime(artifact.policy_path, artifact.manifest.policy.input, artifact.manifest.policy.output) + policy = OnnxPolicy(artifact.policy_path, artifact.manifest.policy.input, artifact.manifest.policy.output) np.random.seed(seed) env = registry.make(env_name, num_envs=1) env.cfg.noise_config.level = 0.0 diff --git a/motrix_rl/tests/test_deploy_export.py b/motrix_rl/tests/test_deploy_export.py index ac11c0b6..9ffab614 100644 --- a/motrix_rl/tests/test_deploy_export.py +++ b/motrix_rl/tests/test_deploy_export.py @@ -28,14 +28,14 @@ class TestDeployTask(DeployTask): __test__ = False spec_type = TestTaskSpec - def reset(self, state, context) -> None: + def reset(self, state) -> None: pass - def build_observation(self, state, context): + def step(self, state, context): raise NotImplementedError - def process_action(self, action): - raise NotImplementedError + def request_stop(self) -> None: + pass def validate_command(self, command) -> None: pass @@ -67,6 +67,7 @@ def _profile() -> DeploymentProfile: ), task=TestTaskSpec(), control=ControlSpec(period_s=0.02, state_timeout_s=0.1), + payloads={"payloads/motion.npz": b"motion-clip-npz-bytes"}, observation_size=4, action_size=2, ) @@ -137,6 +138,10 @@ def test_deployment_export_injects_profile_builder_selected_by_run_metadata( 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" + # Task payloads are declared in the manifest and written next to the policy. + (payload,) = result.artifact.manifest.payloads + assert payload.path == "payloads/motion.npz" + assert (result.artifact.root / "payloads" / "motion.npz").read_bytes() == b"motion-clip-npz-bytes" assert result.artifact.policy_path.read_bytes() == model.model_bytes assert result.validation_samples == 8 assert not checkpoint.with_name("policy.onnx").exists() diff --git a/wiki/design/deploy-command-input.md b/wiki/design/deploy-command-input.md index 2baa60cc..096894cd 100644 --- a/wiki/design/deploy-command-input.md +++ b/wiki/design/deploy-command-input.md @@ -22,11 +22,11 @@ runtime,因此 batch 维度是 command contract 的一部分。 | 契约 | 数据方向 | 语义 | | --- | --- | --- | -| 高层 command | input mechanism → policy task | 策略要完成的目标,例如 `PlanarVelocityCommand` | -| `RobotCommand` | policy task → robot backend | 关节位置、速度、力矩和 gains 等低层执行命令 | +| 高层 command | input mechanism → task/controller | 策略要完成的目标,例如 `PlanarVelocityCommand` | +| `RobotCommand` | controller → robot backend | 关节位置、速度、力矩和 gains 等低层执行命令 | 本文只定义 input mechanism 到高层 command 的路径。observation 拼接、policy inference 和 action processing 仍由 -[Motrix Deploy 框架设计](./motrix-deploy.md) 中的 `DeployTask`、`PolicyRuntime` 和 `RobotInterface` 负责。 +[Motrix Deploy 框架设计](./motrix-deploy.md) 中的 task-owned `PolicyProcessor`、`PolicyController` 与 `Policy` 负责,机器人 I/O 属于 `ControlSession` 与 `RobotInterface`。 ## 2. 最小概念模型 @@ -51,7 +51,7 @@ range + RNG ──> RandomPlanarVelocityBinding ──> PlanarVelocityComm 1. `InputDevice` 不得 import 或构造 command。 2. command dataclass 不得包含 key code、gamepad axis 或设备实现。 3. device 与 command 的组合只出现在 concrete device-backed binding 中;binding 本身不要求 device。 -4. `ControlLoop` 只调用 `CommandBinding.read_command(batch_size=1)`,不包含设备或任务分支。 +4. runtime 只调用 `CommandBinding.read_command(batch_size=1)`,不包含设备或任务分支。 5. Go2 使用的通用平面速度 command 与 device/constant bindings 放在 `motrix_env_core`;带训练采样策略的 binding 放在 `motrix_envs` 对应任务模块。 @@ -146,7 +146,7 @@ command scale 属于 binding。第一版先定义 ABC;具体 provider 等实 ### 4.1 不定义公共 Command ABC -command 是普通 immutable dataclass,不继承公共 base。`CommandT` 只用于 `CommandBinding`、`PolicyContext` 和 +command 是普通 immutable dataclass,不继承公共 base。`CommandT` 只用于 `CommandBinding`、`ControlContext` 和 `DeployTask` 之间的静态类型关联。 是否放入 core 由语义复用范围决定,而不是由运行时继承层次决定。当前设计只增加 Go2 所需的公共平面速度类型。 @@ -321,26 +321,43 @@ physics/control step 无条件重新采样。partial reset 时传入 `batch_size ## 6. Runtime 组合 -`PolicyContext` 只携带 control-loop metadata 与具体 command: +执行层级为 Runtime → ControlSession → DeployTask → phase controllers。Policy(`Policy`)只表示 observation → action;DeployTask 是完整任务 Controller,将机器人状态与控制输入映射为 `RobotCommand`,并编排具体任务需要的 Prepare/Policy/Stop 阶段。`PolicyController` 通过 `PolicyProcessor` 适配模型,不反向引用完整 task。 + +`ControlSession.period_s` 是唯一执行周期来源,Runtime 据此调度;`ControlContext.dt_s` 携带该周期,`elapsed_time_s` 携带连续执行时间。Controller 在阶段开始时记录时间原点以形成局部时钟,不另设执行周期。Task/artifact 的参考采样周期只定义参考数据语义,不承担调度。 + +`ControlContext` 携带 control-loop metadata 与具体 command: ```python @dataclass(frozen=True) -class PolicyContext(Generic[CommandT]): +class ControlContext(Generic[CommandT]): step: int elapsed_time_s: float - command: CommandT + command: CommandT | None + dt_s: float ``` 每个 tick 的 command input 路径只有两步: ```text -1. command = command_binding.read_command(batch_size=1) -2. context = PolicyContext(step=step, elapsed_time_s=elapsed_time_s, command=command) +1. command = command_binding.read_command(batch_size=1) if command_binding is not None else None +2. context = ControlContext(step=step, elapsed_time_s=elapsed_time_s, dt_s=session.period_s, command=command) ``` -随后 `DeployTask.validate_command()` 和 `build_observation()` 消费同一个 command。binding/device 异常统一作为 -`input_error` 结束 rollout;`KeyboardInterrupt` 映射为 `interrupted`。更细的 timeout、disconnect、stale input -错误等真实设备需要区分时再加入。 +ControlSession 的 `command_binding` 默认 `None`;有 binding 时读取指令,没有时直接将 +`ControlContext.command` 设为 `None`。G1 自主播放不创建占位 binding;Go2 仍需外部速度 +binding,并由 task 语义校验拒绝缺失指令,不回退到零速度。Task 不提供 binding factory。 +CLI 从 task 的速度范围组装输入;无速度输入的自主任务校验 `None` 并拒绝 command 配置, +不引入额外 capability 或 registry。 +ControlSession 负责读取可选 binding、构造 context、backend open/enable、状态校验、命令写入、清理和统计。 +`DeployTask.step(state, context)` 消费实测状态和输入,返回 `ControllerStep`; +其 `PolicyController` 通过 `PolicyProcessor` 调用 command 校验、observation 构造、模型推理与动作转换, +不持有 backend handle。`G1WbtDeployTask` 自行管理 ready、policy 与 damping,不让 ControlSession 或 Runtime 解释阶段名;DeploymentRuntime 负责会话调度、异常处理、清理与宿主生命周期;物理周期配置、推进、时间测量、viewer 与仿真世界资源仅属于 SimulationRuntime。 +binding/device 异常统一作为 `input_error` 结束 rollout;`KeyboardInterrupt` 映射为 `interrupted`。 +更细的 timeout、disconnect、stale input 错误等真实设备需要区分时再加入。 + +`ControllerStep.policy_tick` 只标记实际模型推理/actor 决策,不计 ready 或 damping 命令。根 `ControllerStep.error` 对应 `controller_error`;`policy_error` 只用于推理失败。Session 提供 `ControlContext.dt_s` 与 `elapsed_time_s`;Controller 不持有独立执行周期。 + +`ControlSession.result()` 只返回通用 RolloutResult:success、exit reason、模型完成步数、wall time、overrun、trace、error 与 latency。硬件结果不携带仿真指标。SimulationRuntime 返回 SimulationRolloutResult,增加 policy_simulation_time_s(模型命令下实际推进的物理时间,含故障前部分推进,不含 ready/damping)及其与 wall time 的比值 real_time_factor。该指标不是整个世界时钟;通用循环只通过窄的内部宿主 interval hook 执行宿主工作,不识别 physics。 仿真 realtime/fixed-step scheduler 由 `runtime.realtime` 选择,`null` 跟随 `runtime.viewer`;硬件内部始终实时调度,不从 device capabilities 推断。 @@ -349,8 +366,15 @@ class PolicyContext(Generic[CommandT]): `KeyboardDeviceProvider` 与 `GamePadDeviceProvider` 定义在 `motrix_deploy.robot.interface`,是可选输入设备 capability protocol,不是 runtime 插件 factory;返回设备的生命周期由 provider 管理。 deploy 应用在 runtime factory 返回 prepared host 后,从 runtime 的可选 `KeyboardDeviceProvider` 取得 GLFW keyboard device 并创建 binding, -再显式构造 `ControlSession(robot=runtime.robot, ...)`,通过 `runtime.bind_control_session(control)` 绑定; -`with runtime` 打开资源后调用 `runtime.run(steps=...)`。Runtime factory 不承担控制器构造。应用 +在外部加载 Policy,再以 `task = create_task(spec, runtime.robot.spec, policy, steps=50)` 注入任务。 +应用创建 `session = ControlSession(robot=runtime.robot, controller=task, command_binding=binding, period_s=0.02, state_timeout_s=0.1)`, +Task 满足构造时必需的 Controller 契约;调用 `runtime.bind_session(session)`, +通过 `runtime.session` 与 `session.controller` 访问会话及 task,在 `with runtime` 中执行 `runtime.run()`。 +Task 负责自己的阶段编排,通用 DeployTask 不强制阶段顺序;正常停止由 task 决定,故障立即走 backend fallback。 +现有 `motrix_deploy.tasks` entry points 直接提供 task class;`load_task_type` 选择 task 与 spec codec, +`create_task(spec, robot, policy, steps=None)` 构造完整任务,`default_duration_s(spec)` 无需构造 task 即可读取预算。 +PolicyController 持有 `policy_io` 与 Policy,不持有完整 task,避免循环依赖。模型预算在 task 构造时传入, +不传给 run。Runtime factory 不承担任务或控制器构造。应用 不建立 input registry;headless CLI 使用 `command.velocity` 配置 constant command。Go2 task 从 artifact 读取 `command_lower`、`command_upper` 和与 `[vx, vy, yaw_rate]` 对齐的三维 `command_scale`,向 factory 暴露逐元素 `range * scale` 后的映射端点;scale 属于 artifact 的 task contract,不在 runtime recipe 重复配置。内置交互式 @@ -382,7 +406,7 @@ motrix_env_core/src/motrix_env_core/ └── bindings.py # CommandBinding、built-in bindings motrix_deploy/src/motrix_deploy/ -└── runtime/ # PolicyContext;调用 CommandBinding.read_command() +└── runtime/ # ControlContext;调用 CommandBinding.read_command() motrix_deploy_tasks/src/motrix_deploy_tasks/ └── tasks/ @@ -441,7 +465,7 @@ contract,并在四足训练模块中实现带训练分布语义的 random bind ### 9.5 Runtime integration - fake `CommandBinding` 可以驱动 control loop,不需要真实 device; -- 替换 keyboard、gamepad 或 constant binding 不修改 `ControlLoop`、`DeployTask` 或 backend; +- 替换 keyboard、gamepad 或 constant binding 不修改 `Controller`、`DeployTask` 或 backend; - training 使用 random binding 不依赖 `InputDevice` 或 deploy runtime; - Go2 task 直接消费公共 `PlanarVelocityCommand`; - binding 异常产生 `input_error`,用户中断产生 `interrupted`。 diff --git a/wiki/design/motrix-deploy.md b/wiki/design/motrix-deploy.md index eb2dcc83..1ffab01c 100644 --- a/wiki/design/motrix-deploy.md +++ b/wiki/design/motrix-deploy.md @@ -6,7 +6,7 @@ sim2real vertical slice。`motrix_deploy` 是独立于训练框架的策略部署包:训练侧先通过统一的 [ONNX policy exporter](./onnx-export.md) 把 metadata-backed run 转换为经过 parity 验证的自包含 ONNX, 再由 `motrix_envs.deploy` compiler 补全 robot、task 和 control 契约并写出 artifact。部署侧使用带版本的 -`DeployTask`、`PolicyRuntime` 和通用控制循环驱动仿真或真实机器人。运行时 command input 的 +`DeployTask`、`Policy` 和通用控制循环驱动仿真或真实机器人。运行时 command input 的 `InputDevice`、公共 `PlanarVelocityCommand` 和 `CommandBinding` 契约由独立的 [Deploy Runtime Command Input 分层设计](./deploy-command-input.md) 定义。sim2sim 与 sim2real 只替换 `RobotInterface`,不得分别维护 policy tensor 拼接、command mapping 或 action 后处理逻辑。 @@ -38,12 +38,12 @@ motrix_rl checkpoint + run metadata + task snapshot CommandBinding.read_command(batch_size=1) -> PlanarVelocityCommand | v - PolicyContext + ControlContext | - RobotState ----------------------------+----> DeployTask.build_observation + RobotState ----------------------------+----> PolicyProcessor.build_observation | v - DeployTask -> PolicyRuntime -> DeployTask.process_action + DeployTask -> Policy -> PolicyProcessor.process_action ^ | | v +----- RobotInterface <---------- RobotCommand @@ -190,10 +190,10 @@ motrix_deploy/ │ ├── profile.py # DeploymentProfile、compiler registry 和统一 build 入口 │ ├── task.py # DeployTask 接口与版本化 task entry-point discovery │ ├── env.py # robot-free 世界 discovery 与 assemble_deploy_scene(env ID, robot ID) -│ ├── policy/ # PolicyRuntime 与 runtime registry -│ ├── runtime/ # PolicyContext、lifecycle、control loop、scheduler、result/metrics -│ │ ├── base.py # DeploymentRuntime / SimulationRuntime、bind_control_session -│ │ ├── control.py # ControlSession:共享 start/tick/stop +│ ├── policy/ # Policy 与 runtime registry +│ ├── runtime/ # ControlContext、lifecycle、control loop、scheduler、result/metrics +│ │ ├── base.py # DeploymentRuntime / SimulationRuntime、bind_session +│ │ ├── control.py # ControlSession 执行器与 PolicyController 策略控制器 │ │ ├── hardware.py # HardwareRuntime:墙钟 scheduler │ │ └── factory.py # motrix_deploy.backends discovery;返回 prepared runtime │ ├── robot/ # robot I/O 与可选 input-device capability;精简 initializer @@ -223,7 +223,7 @@ motrix_deploy_unitree/ motrix_deploy_tasks/ ├── src/motrix_deploy_tasks/ │ ├── tasks/ -│ │ └── go2_walk.py # Go2WalkTaskSpec 与 Go2WalkDeployTaskV1 同置 +│ │ └── go2_walk.py # Go2WalkTaskSpec 与 Go2WalkDeployTask 同置 │ ├── envs/ # flat.py / rough.py:独立、robot-free 部署世界 │ ├── config/task/ # go2-walk-flat/{sim,hardware}.yaml、go2-walk-rough/sim.yaml │ └── __init__.py # 精简 package initializer;task 类由 entry point 声明 @@ -282,28 +282,26 @@ class RobotState: base_orientation_xyzw: np.ndarray base_angular_velocity: np.ndarray base_linear_acceleration: np.ndarray - base_position: np.ndarray | None = None base_linear_velocity: np.ndarray | None = None - extras: Mapping[str, np.ndarray] = field(default_factory=dict) ``` - joint 数组使用 `RobotSpec.joint_names` 顺序。 - `sample_time_ns` 表示底层采样时间;没有可靠设备时钟时可等于本机接收时间。 - `receive_time_ns` 使用本机单调时钟,用于 timeout 判断。 -- optional 字段只有在 observation 或 metric 需要时才成为 backend 必需能力。 -- `extras` 用稳定名称承载足底力、里程计等扩展;artifact 必须声明实际消费的 key、shape、dtype 和单位。 +- `base_linear_velocity` 是可选 world-frame sensor/estimator 输出;当前 Unitree 接口不提供,样本保持 `None`,Go2/G1 部署 task 不依赖它。 +- world-frame base position 不属于通用 `RobotState`;Go2 fall termination 使用 IMU 姿态,不使用世界高度。仿真 ground truth 只能保留在 simulator-specific diagnostics。 ### 6.3 Command input 机器人状态不是 observation 的唯一输入。Go2 速度跟踪策略还需要平面目标速度。runtime 使用以下单向数据流: ```text -CommandBinding.read_command(batch_size=1) -> PlanarVelocityCommand -> PolicyContext -> DeployTask +CommandBinding.read_command(batch_size=1) -> PlanarVelocityCommand -> ControlContext -> DeployTask ``` 最小 device/binding 接口、包归属、配置和测试边界统一由 [Deploy Runtime Command Input 分层设计](./deploy-command-input.md) 定义。本框架只要求 `ControlLoop` 调用已经装配的 -binding,不直接读取 device,也不解释 `PlanarVelocityCommand` 字段。公共 command 始终携带显式 batch 维;当前 +可选 binding;未提供 binding 的自主控制会话直接将 `ControlContext.command=None` 交给 controller,不创建占位输入对象。Go2 等需要外部输入的 task 在输入边界拒绝缺失 command,不能静默补零。Session 不直接读取 device,也不解释 `PlanarVelocityCommand` 字段。公共 command 始终携带显式 batch 维;当前 deploy runtime 只接受 `values.shape == (1, 3)`,Go2 task 校验 batch size 后取出唯一一行。 ### 6.4 RobotCommand @@ -325,7 +323,7 @@ tau = kp * (q_target - q) + kd * (qd_target - qd) + tau_ff ``` backend 可以把该命令传给原生 hybrid controller,也可以在本地计算 torque,但必须声明相应 capability。 -`DeployTask.process_action()` 在写入 backend 前完成 action 和目标 position 裁剪,并生成显式 gain。backend 仍需执行最后 +`PolicyProcessor.process_action()` 在写入 backend 前完成 action 和目标 position 裁剪,并生成显式 gain。backend 仍需执行最后 一道设备级硬限制,不能扩大 artifact 的限幅。 ### 6.5 RobotInterface 与 capability @@ -360,7 +358,7 @@ class RobotInterface(ABC): `RobotCapabilities` 至少声明: - 支持的控制模式; -- 可用 state 字段和 extra sensors; +- 支持的 state 字段; - 是否支持渲染; - 最大推荐 command rate; - 是否需要显式 enable,以及 stop 的安全语义。 @@ -368,8 +366,28 @@ class RobotInterface(ABC): 仿真 backend 在 `open()` 中完成自身的确定性初始化;真机 backend 建立通信后由 `read_state()` 返回当前状态。 `enable()` 不接收 initial command;Unitree backend 自行构造到 `RobotSpec.default_joint_position` 的姿态过渡, 目标速度与 feedforward torque 为零,使用解析后的 `runtime.kp` / `runtime.kd`,`null` 时保留 artifact 增益。 -不通过 task action 推导启动命令。`ControlSession.start()` 不调用 task `process_action()` 生成零动作启动命令; -初始状态的 safety/termination 检查仍先于 enable,Start/A 门控与 enable 后新状态检查保持有效。 +不通过 task action 推导启动命令。Runtime 不调用 task `process_action()` 生成零动作启动命令; +无 task 准备阶段时,初始状态的 termination 检查先于 enable;有准备阶段时使用 enable 后的实测状态准备,准备完成后才检查 policy 接管条件。Start/A 门控保持有效。 + +#### Runtime 驱动的控制生命周期 + +Policy 专指 observation → action 的映射,模型运行接口为 `Policy`。Controller 指实测 `RobotState` + 外部输入 → `RobotCommand`,控制级 `Controller` 返回 `ControllerStep` 命令、完成状态及指标;共享输入上下文为 `ControlContext`。 +DeployTask 是完整任务控制入口,满足 Controller 契约,拥有任务阶段和正常停止逻辑。应用加载 Policy 后注入 task 构造函数;Session 直接执行 task。G1 task 按 `prepare → policy_hold → playback → damping` 编排,对齐真实部署流程;Go2 task 内部委托 PolicyController,通用 Task 不固定阶段序列,不引入状态机 registry 或 DSL。 +`PolicyController` 包装独立 PolicyProcessor 的 observation/action 语义与 Policy 推理,只生成命令,不持有完整 Task、robot handle 或 backend;Task → PolicyController → PolicyProcessor 避免 Task 与策略控制器的循环依赖。prepare 与 damping 是确定性 controller,不伪造 observation/action。 + +G1 四阶段语义: + +- **prepare**:从实测关节姿态 ramp 到独立站立姿态(robot 默认站立位,非 motion 第 0 帧),使用 artifact 中独立的 preparation kp/kd(训练增益是动态跟踪增益,过软无法静态保持)与时序(spec 字段 `preparation_ramp_duration_s`/`preparation_settle_duration_s`/`preparation_timeout_s`,profile 编译期定值)。就绪门(误差/速度/倾角连续满足 settle 时长)不变;首次就绪后超时解除但每 tick 重新评估,倾角故障立即失败。就绪后按接管模式决定是否等待:默认自动接管,manual 模式保持等待操作员请求。 +- **policy_hold**:实测就绪且接管请求置位(默认 `automatic_start=True` 时就绪后下一 tick 自动置位;manual 模式由显式 `request_policy_start()` 置位)后,同一份实测状态立即切换;policy 以冻结的 motion 起始帧推理,更新 action 历史,不消耗 playback 预算。至少执行一个 hold 周期后才可能进入 playback。 +- **playback**:motion 参考帧在 hold 驻留目标到达或显式 `request_motion_start()` 后开始推进;policy 历史连续不重置,仅将剩余播放预算设为有限值。playback 步数由 task 单独统计(`playback_steps`),通用 `completed_steps` 包含 hold 推理。舞蹈以后倾姿态结束,只有跟踪中的 policy 能维持平衡,播放预算完成后直接进入 damping;以站立收场需要 motion 本身以站立姿态结束。 +- **damping/complete**:播放预算完成前仍执行最终实测状态检查;正常停止请求立即进入 damping,故障由 Session 兜底绕过正常停止。 + +操作员输入是任务语义,由 task 自身响应,CLI 不路由任何任务特有选项(`DeployRunConfig` 不含任务选项通道)。G1 task 构造时以 `automatic_start` 选择接管模式:默认 `True`,实测就绪后下一 tick 自动置位接管请求;`False` 为 manual 模式,阶段切换完全由请求方法驱动。程序化应用可通过 `attach_keyboard(keyboard, hold_ticks=...)` 接入 viewer 键盘(`p` 接管 / `m` 提前播放 / `o` 停止,在 `step()` 开头翻译为内部请求标志)或 `enable_automatic_start(hold_ticks=...)` 脚本化驻留接管。请求只置位,实际切换在下一个控制 tick 由 task 依据实测就绪状态决定;不引入通用 phase command registry。 +采用 Runtime → ControlSession → Controller:Runtime 持有世界/宿主、scheduler、物理推进和 viewer,bind_session(session);阶段无关的 ControlSession 表示一次控制执行会话,构造时接收必需的 Controller,持有机器人 I/O、binding、状态与命令边界校验、命令写入、故障停机及统计。Session 不存在未绑定 controller 的中间状态,不识别 ready/model/damping,不创建 task controller,不自行等待或推进物理。DeploymentRuntime 的共享循环只通过 `_check_running()` 检查宿主是否允许继续,并通过窄的 `_advance_interval()` 执行宿主区间扩展;不查询或累计物理时间。物理推进、physics timestep 比例校验、物理时钟与 viewer 检查接口属于 SimulationRuntime,HardwareRuntime 不暴露这些仿真方法。 +现有任务插件 entry point 直接关联 DeployTask 类型,codec 使用 task_type.spec_type;CLI 调用 create_task(spec, robot_spec, policy, steps=...) 得到完整任务控制入口,不需要外部 controller builder 或插件描述对象。Controller 契约不要求持有 Policy:纯确定性 controller 同样可由 ControlSession 执行。 +子 controller 完成时,复合 controller 用同一份实测状态切换并执行下一 controller,不消耗空周期;最终 controller 完成后退出。每个未完成的控制决策经历一个控制周期。阶段诊断仅由具体复合 controller 持有,不进入 ControllerStep、ControlSession 或通用 RolloutResult;通用执行器不透传状态机历史。 +最后一个模型命令作用周期结束后,PolicyController 检查实际状态,再转入 damping。准备/停止命令不计 policy budget。执行 period_s 仅由 ControlSession 持有,并通过 ControlContext.dt_s 与 elapsed_time_s 提供给 controller;Controller 不保存执行周期、不按 steps × 自己的周期重算时间。各子 controller 以进入时刻为原点建立局部时钟,history 在进入时归零,runtime deadline 连续。Task/artifact 的参考采样周期保留为模型或 motion 数据语义,不是 controller 的调度配置。 +故障由 ControlSession 执行 backend 停机,Runtime 清理世界/宿主资源,跳过正常 damping;正常停止请求由复合 controller 决定具体转换。Controller 不保存 runtime 回调,不创建 scheduler 或调用 sleep。 `stop()` 和 `close()` 必须幂等,任何启动后异常都按 `stop -> close` 收尾。`HealthStatus` 包含状态、原因和最后 成功通信时间,不能只返回无语义的 bool。 @@ -379,39 +397,53 @@ class RobotInterface(ABC): class DeployTask(ABC, Generic[CommandT]): spec_type: ClassVar[type[TaskSpec]] - def reset(self, state: RobotState, context: PolicyContext[CommandT]) -> None: ... - def build_observation(self, state: RobotState, context: PolicyContext[CommandT]) -> np.ndarray: ... + @classmethod + def default_duration_s(cls, spec: TaskSpec) -> float | None: ... + def reset(self, state: RobotState) -> None: ... + def step(self, state: RobotState, context: ControlContext[CommandT]) -> ControllerStep: ... + def request_stop(self) -> None: ... + def validate_command(self, command: CommandT | None) -> None: ... + +class PolicyProcessor(ABC, Generic[CommandT]): + @abstractmethod + def reset(self, state: RobotState, context: ControlContext[CommandT]) -> None: ... + @abstractmethod + def build_observation(self, state: RobotState, context: ControlContext[CommandT]) -> np.ndarray: ... + @abstractmethod def process_action(self, action: np.ndarray) -> RobotCommand: ... - def validate_command(self, command: CommandT) -> None: ... + @abstractmethod + def validate_command(self, command: CommandT | None) -> None: ... + def check_termination(self, state: RobotState) -> str | None: + return None ``` artifact 的 task wire format 只记录版本化 task 标识和 config;内存中的 `TaskSpec` 是具体任务的 Pydantic model, 其 codec 与 class discovery 见 [Manifest 内容](#82-manifest-内容)。observation/action 维度只记录在 `policy.input/output.shape`。 `DeployTask` 不声明 `observation_size` 或 `action_size`;实际 observation 数组由 policy 输入边界校验, -实际 action 数组由 task 的 `process_action()` 边界校验,CLI 不提前比较 task 维度。具体 observation 顺序、 +实际 action 数组由 PolicyProcessor 的 `process_action()` 边界校验,CLI 不提前比较 task 维度。具体 observation 顺序、 previous action、phase 与 action 后处理算法由 `motrix_deploy_tasks` 中一个带版本的 task 类直接实现,不在 manifest 中构造通用计算 DSL。修改已有语义必须新增 task version,不能静默改变旧实现。部署时关闭训练 observation noise; 训练框架的 observation normalizer 必须烘焙进 ONNX。 -### 6.7 PolicyRuntime +### 6.7 Policy ```python -class PolicyRuntime(ABC): +class Policy(ABC): def reset(self) -> None: ... def infer(self, observation: np.ndarray) -> np.ndarray: ... ``` -第一版内置 `OnnxPolicyRuntime`,使用确定性 action 输出。runtime 创建时验证模型输入输出名称、dtype 和 shape; +第一版内置 `OnnxPolicy`,使用确定性 action 输出。runtime 创建时验证模型输入输出名称、dtype 和 shape; 每次 `infer()` 验证输出无 NaN/Inf。接口保留 `reset()` 以支持未来 recurrent policy,但第一版 artifact 只接受 单输入、单输出、无隐藏状态的 ONNX policy。 -`DeployTask.build_observation()` 的输出是训练环境的原始 actor observation。训练框架的 observation normalizer 和 deterministic +`PolicyProcessor.build_observation()` 的输出是训练环境的原始 actor observation。训练框架的 observation normalizer 和 deterministic policy head 必须烘焙进 ONNX,不能在 runtime 外保留 framework-specific preprocessor;不满足自包含约束的模型 拒绝导出为第一版 artifact。 ### 6.8 Task-specific action -第一版 `go2_walk/v1` 在自己的 `DeployTask` 中实现 joint-position target:raw action 经过 finite check、clip、 +第一版 `go2_walk/v1` 在自己的 `Go2WalkPolicyProcessor` 中实现 joint-position target:raw action 经过 finite check、clip、 per-joint scale 和 default pose offset,再应用 position limit,生成零目标速度、零 feed-forward torque 和配置的 KP/KD。task 同时保存 previous action;sim2sim 与 sim2real 必须复用同一版本化 task 标识及 config。 @@ -430,13 +462,13 @@ event-based device 执行一次 `poll()`。`ControlLoop.run()` 的单步顺序 ```text 1. LoopScheduler 到达当前 tick deadline -2. CommandBinding.read_command(batch_size=1) 产生 PlanarVelocityCommand,并构造经过 task 校验的 PolicyContext +2. CommandBinding.read_command(batch_size=1) 产生 PlanarVelocityCommand,并构造经过 task 校验的 ControlContext 3. 从 RobotInterface 读取 RobotState,检查 backend health、状态时效、shape 和 finite values -4. DeployTask.build_observation(state, context) -5. PolicyRuntime.infer(obs) -6. DeployTask.process_action(action) -7. safety limiter 验证 RobotCommand,RobotInterface.write_command(command) -8. 记录 input / binding / read / obs / inference / action / write / loop timing +4. DeployTask.step(state, context) 选择当前 controller 并返回 ControllerStep + policy phase 内部执行 PolicyProcessor.build_observation → Policy.infer → PolicyProcessor.process_action +5. Session 验证 RobotCommand,RobotInterface.write_command(command) +6. Runtime 执行宿主区间并等待下一 deadline(仿真推进物理,硬件仅等待) +7. 记录 input / read / observation / inference / action / write / loop timing ``` `ControlLoop` 接收 `LoopScheduler`,但不判断当前是否为仿真: @@ -445,7 +477,7 @@ event-based device 执行一次 `poll()`。`ControlLoop.run()` 的单步顺序 - `FixedStepScheduler` 每次推进 artifact 定义的固定 `dt`,不执行 wall-clock sleep,用于默认 headless sim2sim 和确定性 CI。 -两种 scheduler 向 `PolicyContext` 提供相同的 step 和 elapsed control time;wall-clock latency 单独计量。 +两种 scheduler 向 `ControlContext` 提供相同的 step 和 elapsed control time;wall-clock latency 单独计量。 仿真的 realtime/fixed-step scheduler 由 `runtime.realtime` 选择;`null` 跟随 `runtime.viewer`。硬件始终在内部实时调度,不从 device 推断。 Fixed-step 模式记录“若实时运行是否 overrun”,但默认不因此终止 rollout。第一版 policy rate 与 command rate 相同;未来需要多速率时通过 scheduler 扩展,不能由 backend 私自重复或插值 policy action。 @@ -541,10 +573,10 @@ manifest 至少包含以下部分。下面是结构节选,为便于阅读省 frozen 字段赋值和有限数值约束;提供公共 `to_dict()` / `from_dict()` codec。具体 spec 声明 `task_name: ClassVar[str]`,通过 `Annotated` / `Field` 约束字段,通过 `model_validator` 约束跨字段关系。 构造与 `model_validate()` 自动校验,不另设手写 `validate()` 或 dataclass `__post_init__`。 -- `Go2WalkTaskSpec` 与 `Go2WalkDeployTaskV1` 同置于 `motrix_deploy_tasks.tasks.go2_walk`;task 类通过 +- `Go2WalkTaskSpec` 与 `Go2WalkDeployTask` 同置于 `motrix_deploy_tasks.tasks.go2_walk`;task 类通过 `spec_type = Go2WalkTaskSpec` 关联配置类型。`action_scale` 是按 canonical joint order 排列的 `list[float]` 向量;训练侧 compiler 在导出时把 scalar scale 展开为逐关节向量,codec 不接受 scalar fallback。 -- manifest 读取按 `task.name` 加载唯一 `motrix_deploy.tasks` entry point 指向的 task **类**,不构造 runtime task, +- manifest 读取按 `task.name` 加载唯一 `motrix_deploy.tasks` entry point 的 entry point 关联的 task **类**,不构造 runtime task, 再按其 `spec_type.model_validate()` 解码、校验具体 model。只使用这一组 task entry points,不增加独立 spec entry point, 也不保留原始 dict fallback。未知或重复插件、未知字段、缺少必需字段和非法类型在读取边界拒绝。 @@ -574,7 +606,7 @@ deployment export 不得再按 `(rllib, train_backend, algo)` 实现 `export_rsl task/robot/action 语义时,在 `motrix_envs.deploy` 增加 profile compiler,在 `motrix_deploy_tasks` 增加 task 实现, compiler 通过 import-time 注册接入 profile registry,并直接构造该 task 的具体 spec;task 类通过唯一的 `motrix_deploy.tasks` entry point 接入,如 -`"go2_walk/v1" = "motrix_deploy_tasks.tasks.go2_walk:Go2WalkDeployTaskV1"`,由类上的 `spec_type` 同时支持 +`"go2_walk/v1" = "motrix_deploy_tasks.tasks.go2_walk:Go2WalkDeployTask"`,由类上的 `spec_type` 同时支持 manifest 解码和运行时构造,不另设 spec 插件。 组合导出接口直接消费经过验证的 ONNX model bytes 和 `OnnxExportReport`。独立导出 `.onnx` 的用户入口继续 @@ -849,7 +881,7 @@ profile compiler registry;`motrix_envs.deploy` import 具体 compiler 时通 `register_profile_compiler()` 装饰器按环境名自动注册。task runtime 通过 `motrix_deploy.tasks` Python entry-point group 按版本化 task 标识发现。`available_tasks()` 只读取已安装插件元数据;`load_task_type()` 懒加载唯一 `DeployTask` 类,manifest codec 读取其 `spec_type` 而不实例化 task。`create_task()` 再将已解码的具体 -`TaskSpec` 和 `RobotSpec` 传给该类构造运行时 task。未知名称或重复插件报错,不按 import 顺序选择实现。 +`TaskSpec`、`RobotSpec`、已加载的 `Policy` 和可选 `steps` 传给该类构造完整 task controller。未知名称或重复插件报错,不按 import 顺序选择实现。 `motrix_deploy` core 发布指向 `motrix_deploy.cli:main` 的 console script,不反向 import 具体环境、task 或 SDK; 具体部署 recipe 通过已安装 Hydra search-path plugin 提供的 `task` group 选择;外部应用也可显式指定 Hydra config path/name。 @@ -862,7 +894,7 @@ policy runtime 由 CLI 装配;observation 与 action 语义由具体 `DeployTa term/processor registry。 注册冲突、未知组件和版本不兼容在 artifact validation 阶段报错。注册的具体实现只获得公共 contract,不获得 -`PolicyRuntime` 或 raw observation,因此无法绕过统一 task 路径。 +`Policy` 或 raw observation,因此无法绕过统一 task 路径。 ## 11. MuJoCo sim2sim 设计 @@ -878,8 +910,8 @@ rollout 支持平地与确定性 procedural height-field 世界,分别通过 ```text CommandBinding.read_command(batch_size=1) -> PlanarVelocityCommand --+ v -MuJoCo state -> RobotState -> Go2WalkDeployTaskV1 -> ONNX PolicyRuntime - -> Go2WalkDeployTaskV1 -> RobotCommand -> MuJoCo torque/actuator +MuJoCo state -> RobotState -> Go2WalkDeployTask -> ONNX Policy + -> Go2WalkDeployTask -> RobotCommand -> MuJoCo torque/actuator ``` 不允许为了尽快跑通而直接调用 `QuadrupedWalkTask._compute_obs()` 或 `apply_action()`;deployment @@ -964,32 +996,30 @@ config = SimulationRuntimeConfig( ``` Runtime factory 只选择并准备宿主,不通过回调构造控制器。创建返回后,`runtime.robot` 及其只读 -`spec` 在 `open()` 之前已可用;应用显式构造自己的 task、policy、binding 和 `ControlSession`: +`spec` 在 `open()` 之前已可用;应用显式构造自己的 task、policy、binding 和控制器: ```python from motrix_deploy.runtime.control import ControlSession +from motrix_deploy.task import create_task from motrix_deploy.runtime.factory import create_simulation_runtime runtime = create_simulation_runtime("mujoco", config) robot = runtime.robot -control = ControlSession( - robot=robot, - task=task, - policy=policy, - command_binding=command_binding, - period_s=0.02, +task = create_task(task_spec, robot.spec, policy, steps=50) +session = ControlSession( + robot=robot, controller=task, command_binding=command_binding, + period_s=0.02, state_timeout_s=0.1, ) -runtime.bind_control_session(control) +runtime.bind_session(session) with runtime: - result = runtime.run(steps=50) + result = runtime.run() ``` 这里的 task、policy 与 command_binding 由应用持有;依赖机器人契约的 task 可在 runtime 创建后从 `robot.spec` 构造。硬件由 `create_hardware_runtime(name, config, context)` 返回 prepared host 后,使用同样的 -显式组装和绑定流程。`run(control, steps=...)` 仍可直接接收 session;绑定和传入的 session 必须属于该 -runtime 的 robot,不能覆盖另一个已绑定的 session。`ControlSession.period_s` 是控制周期的唯一来源; -`SimulationRuntimeConfig` 不重复声明控制周期,仿真宿主在绑定或解析 session 时校验它与 physics timestep 的整数倍关系。 -Task 只由 control session 持有,不重复传给 runtime。 +显式组装和绑定流程。Runtime 只绑定属于自身 robot 的 ControlSession;ControlSession 在构造时接收必需的 controller 与可选输入 binding。G1 自主播放不传 binding;task 不创建输入适配器,应用仅为需要外部命令的 task 组装 binding。 +预算在创建 Task 时传入并交给内部 PolicyController,不重复传给 run。ControlSession.period_s 是调度周期,仿真绑定时验证其为 physics timestep 的整数倍。 +Task 作为 session.controller 执行;PolicyController 只使用 PolicyProcessor。 `motrix_deploy` core 不查询训练环境 registry,也不依赖仿真器。MuJoCo plugin 将完整场景与 `config.physics` 直接传给 `MuJoCoSceneCompiler.create_spec()`,编译 terrain、robot、sensor 与视觉配置, @@ -1028,14 +1058,14 @@ sim2sim 的 Hydra 配置将 artifact、duration_s 和 command 放在顶层,仿 - 使用 task 从 artifact 读取的 command range 乘以 scale 后创建 keyboard binding; - reset 后从 step 0 重新初始化 phase、previous action 和 scheduler; - 用户中断时受控停止; -- 输出 JSON-compatible `RolloutResult`; -- 可配置任务指标:base height、fall、速度跟踪误差等,但这些指标不进入通用控制循环。 +- 硬件输出通用 `RolloutResult`:成功状态、退出原因、完成步数、墙钟时间、overrun、trace、错误和 latency;ControlSession 只构造此通用结果; +- 仿真输出 `SimulationRolloutResult`,额外携带 `policy_simulation_time_s` 和 `real_time_factor`。前者只累计模型命令作用期间实际推进的物理时间(包括失败前的部分区间),不包含 ready/damping;后者为此时间与墙钟运行时间之比。SimulationRuntime 独立累计并添加这些指标,硬件不生成仿真指标; +- 可配置任务指标:IMU fall、速度跟踪误差等,但这些指标不进入通用控制循环,也不引入不可观测的世界高度。 deployment backend 使用最小 GLFW viewer:每次 `sync()` 只更新 MuJoCo scene 并 render,不拥有仿真步进;同一 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 配置。 +`runtime.realtime=null` 时跟随 `runtime.viewer`,也可显式配置实时节奏。程序化确定步数运行在 Task 构造时传入 `steps=...` 设置内部模型控制器预算,随后由 `runtime.run()` 执行。 ### 11.5 原生 MotrixSim runtime @@ -1048,8 +1078,8 @@ motrix-deploy task=go2-walk-flat/sim artifact=artifacts/go2-walk-flat.deploy \ ``` 粗糙地形同样使用 `task=go2-walk-rough/sim`,不复制 backend-specific recipe。程序化 factory 使用 -`create_simulation_runtime("motrixsim", config)`,返回 prepared `runtime.robot` 后显式构造并绑定 session。 -`ControlSession.period_s` 是唯一控制周期来源,绑定或解析 session 时验证 physics timestep 的整数倍关系。 +`create_simulation_runtime("motrixsim", config)`,返回 prepared `runtime.robot` 后显式构造并绑定 ControlSession。 +绑定 ControlSession 时确定控制周期并验证 physics timestep 的整数倍关系。 `MotrixSimRuntime` 持有 `MotrixSimSceneCompiler` 产生的 MSD world、原生 model/data、reset、physics loop 与 SDK `RenderApp` viewer,不借用 MuJoCo runtime,不实例化训练环境。Native robot port 只读状态、缓存控制目标, @@ -1071,7 +1101,7 @@ receive timestamp 与 realtime pacing 使用 monotonic clock。Native viewer 提 ```text motrix_deploy_motrixsim/src/motrix_deploy_motrixsim/ ├── plugin.py # motrixsim entry point;共享 SimulationRuntimeConfig / artifact context -├── runtime.py # native world/model/data、physics、reset 与 ControlSession 调度 +├── runtime.py # native world/model/data、physics、reset 与 Controller 调度 ├── interface.py # semantic sensor binding、query/write、非 owning RobotInterface └── viewer.py # SDK RenderApp 与 viewer-owned KeyboardDevice ``` @@ -1106,7 +1136,7 @@ Hydra 配置的公共执行字段是 artifact、duration_s 和 command;task 不是嵌套配置映射或 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=...) 保留确定步数运行。 +duration_s 换算为模型 policy 的控制 tick 预算,不是墙钟超时;程序化创建 Task 时用 steps 设置内部模型决策预算,runtime.run() 不重复接收步数。 具体 task/runtime recipe 属于应用层,由 `motrix_deploy_tasks` 插件安装,提供 flat/rough 场景、机器人和 command 默认值。 `motrix_deploy` 包内保留通用配置;任务 defaults 选择 runtime/sim 或 runtime/hardware 配置组。 @@ -1149,7 +1179,7 @@ deployment-enabled task 必须在 `motrix_deploy_tasks` 提供一个带版本的 标识和 config。profile 单独保留编译环境的 observation/action 维度供导出校验,不写入 task serialization; artifact 的维度以 policy tensor shapes 为唯一来源。compiler 不恢复 policy checkpoint,也不执行 ONNX 导出。 每个 task version 至少有一组 golden state/context/action probe, -同时在训练环境和具体 `DeployTask` 上运行,断言 observation 和 command 数值一致。 +同时在训练环境和具体 `PolicyProcessor` 上运行,断言 observation 和 command 数值一致。 这项一致性检查防止训练环境继续演进后 deployment profile 静默落后。reward、critic observation 和训练噪声 不属于部署 profile;训练框架的 observation normalizer 已由 policy exporter 烘焙进 ONNX,`DeployTask` @@ -1219,7 +1249,7 @@ Go2 vertical slice 的验收基线为:确定性初始化的 1000-tick headless 1. 建立包含通用 CLI 的 `motrix_deploy` library,以及通过 `motrix_deploy.tasks` entry points 提供具体任务类的 `motrix_deploy_tasks` workspace package。 2. 实现公共 contracts、artifact schema/reader/validator 和 component registry。 -3. 实现 `DeployTask` 接口、ONNX `PolicyRuntime` 和直接编码 obs/action 的 `go2_walk/v1`。 +3. 实现 `DeployTask` 接口、ONNX `Policy` 和直接编码 obs/action 的 `go2_walk/v1`。 4. 实现统一 control loop、scheduler、metrics、fake backend 与 contract tests。 5. 为一个现有 `go2-walk-rough/rslrl.ppo` checkpoint 生成可验证 artifact,并完成训练/部署 golden probe。 6. 实现 entry-point MuJoCo backend plugin、SceneCfg backend config,以及 headless/GLFW-viewer deployment run。