diff --git a/.gitignore b/.gitignore index 9df32a80..f3e78aee 100644 --- a/.gitignore +++ b/.gitignore @@ -15,3 +15,7 @@ docs/source/index.md .worktrees .obsidian /unitree_sdk2_python/ +build/ +dist/ +*.so +*.egg-info/ diff --git a/artifacts/g1-flat.deploy/manifest.json b/artifacts/g1-flat.deploy/manifest.json new file mode 100644 index 00000000..8ee18e8a --- /dev/null +++ b/artifacts/g1-flat.deploy/manifest.json @@ -0,0 +1,375 @@ +{ + "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": [], + "policy": { + "component_version": "onnx/v1", + "input": { + "dtype": "float32", + "name": "obs", + "shape": [ + 1, + 100 + ] + }, + "output": { + "dtype": "float32", + "name": "actions", + "shape": [ + 1, + 29 + ] + }, + "payload_path": "policy/model.onnx", + "sha256": "21d8aa4c77e5ba7ee3876974c5f28223b284da33544921a20286e0a091b2a54c" + }, + "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-14_20-24-17-390551" + }, + "task": { + "config": { + "action_lower": [ + -12.767200469970703, + -11.868399620056152, + -11.030400276184082, + -8.84320068359375, + -3.5464000701904297, + -1.0471999645233154, + -12.767200469970703, + -11.868399620056152, + -11.030400276184082, + -8.84320068359375, + -3.5464000701904297, + -1.0471999645233154, + -10.472000122070312, + -2.0799999237060547, + -2.0799999237060547, + -13.156800270080566, + -8.205999374389648, + -10.472000122070312, + -6.588799953460693, + -7.888879776000977, + -6.457719802856445, + -6.457719802856445, + -13.156800270080566, + -8.205999374389648, + -10.472000122070312, + -6.588799953460693, + -7.888879776000977, + -6.457719802856445, + -6.457719802856445 + ], + "action_scale": [ + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25, + 0.25 + ], + "action_upper": [ + 12.767200469970703, + 11.868399620056152, + 11.030400276184082, + 8.84320068359375, + 3.5464000701904297, + 1.0471999645233154, + 12.767200469970703, + 11.868399620056152, + 11.030400276184082, + 8.84320068359375, + 3.5464000701904297, + 1.0471999645233154, + 10.472000122070312, + 2.0799999237060547, + 2.0799999237060547, + 13.156800270080566, + 8.205999374389648, + 10.472000122070312, + 6.588799953460693, + 7.888879776000977, + 6.457719802856445, + 6.457719802856445, + 13.156800270080566, + 8.205999374389648, + 10.472000122070312, + 6.588799953460693, + 7.888879776000977, + 6.457719802856445, + 6.457719802856445 + ], + "base_angular_velocity_scale": 0.25, + "command_lower": [ + -1.0, + -1.0, + -1.0 + ], + "command_upper": [ + 1.0, + 1.0, + 1.0 + ], + "gait_period_s": 1.0, + "gait_phase_offsets_rad": [ + 0.0, + 3.141592653589793 + ], + "joint_position_scale": 1.0, + "joint_velocity_scale": 0.05, + "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 + ], + "standing_command_threshold": 0.01 + }, + "name": "g1_walk/v1" + } +} diff --git a/artifacts/g1-flat.deploy/policy/model.onnx b/artifacts/g1-flat.deploy/policy/model.onnx new file mode 100644 index 00000000..86c67774 Binary files /dev/null and b/artifacts/g1-flat.deploy/policy/model.onnx differ diff --git a/motrix_deploy/src/motrix_deploy/cli.py b/motrix_deploy/src/motrix_deploy/cli.py index 9dcbf153..a564d103 100644 --- a/motrix_deploy/src/motrix_deploy/cli.py +++ b/motrix_deploy/src/motrix_deploy/cli.py @@ -14,8 +14,9 @@ from omegaconf.errors import OmegaConfBaseException from motrix_deploy.artifact.io import inspect_artifact, read_artifact -from motrix_deploy.config import DeployRunConfig, SimRuntimeCfg +from motrix_deploy.config import DeployRunConfig, ProgramRunConfig, SimRuntimeCfg from motrix_deploy.errors import ArtifactError, ValidationError +from motrix_deploy.operator import OperatorCommandBinding 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, load_task_type @@ -52,6 +53,7 @@ def run(cfg: DeployRunConfig) -> int: device = runtime.keyboard or runtime.gamepad try: manifest.robot.validate_compatible(robot.spec) + task.configure_operator(getattr(cfg.runtime, "operator", None)) command_binding = task.prepare(simulation=simulation, device=device, spawn_pose=spawn_pose) session = ControlSession( controller=task, @@ -99,6 +101,80 @@ def _run_config(cfg: DictConfig) -> None: _hydra_main = hydra.main(version_base=None, config_path="pkg://motrix_deploy.config", config_name="deploy")(_run_config) +def run_program(cfg: ProgramRunConfig) -> int: + """Run one persistent multi-policy program on the selected backend.""" + from motrix_deploy.program import create_program + + program = create_program(cfg.program) + simulation = isinstance(cfg.runtime, SimRuntimeCfg) + if simulation: + runtime = create_simulation_runtime(cfg.runtime.backend, cfg.runtime.build_runtime_config()) + else: + runtime = create_hardware_runtime( + cfg.runtime.backend, + cfg.runtime, + RuntimeCreateContext(robot=program.robot, control=program.control), + ) + try: + program.robot.validate_compatible(runtime.robot.spec) + profile = getattr(cfg.runtime, "operator", None) or program.operator + command_binding = ( + OperatorCommandBinding( + runtime.keyboard or runtime.gamepad, + profile, + command_lower=program.command_lower, + command_upper=program.command_upper, + ) + if (runtime.keyboard or runtime.gamepad) is not None + else program.prepare(None) + ) + session = ControlSession( + controller=program.controller, + robot=runtime.robot, + command_binding=command_binding, + period_s=program.control.period_s, + state_timeout_s=program.control.state_timeout_s, + ) + runtime.bind_session(session) + except (ValidationError, ValueError): + runtime.close() + raise + with runtime: + result = runtime.run() + print(json.dumps(result.to_dict(), indent=2, sort_keys=True)) + return 0 if result.success else 1 + + +def _run_program_config(cfg: DictConfig) -> None: + try: + typed_cfg = OmegaConf.to_object(OmegaConf.merge(OmegaConf.structured(ProgramRunConfig), cfg)) + if not isinstance(typed_cfg, ProgramRunConfig): + raise TypeError(f"Expected ProgramRunConfig, got {type(typed_cfg).__name__}") + exit_code = run_program(typed_cfg) + except (ArtifactError, OmegaConfBaseException, RuntimeError, TypeError, ValidationError, ValueError) as error: + print(json.dumps({"valid": False, "error": str(error)}, sort_keys=True), file=sys.stderr) + raise SystemExit(2) from error + if exit_code: + raise SystemExit(exit_code) + + +_hydra_program_main = hydra.main(version_base=None, config_path="pkg://motrix_deploy.config", config_name="program")( + _run_program_config +) + +_hydra_program_real_main = hydra.main( + version_base=None, config_path="pkg://motrix_deploy.config", config_name="program-real" +)(_run_program_config) + +# deploy= aliases: one flag selects a whole application configuration. +# The simulation application keeps the generic program entry point, so its preset +# also selects the program group option. +_DEPLOY_PRESETS = { + "g1-sim": (_hydra_program_main, ("program=g1-hot-swapping",)), + "g1-real": (_hydra_program_real_main, ()), +} + + def inspect(cfg: DictConfig) -> None: """Validate one deployment artifact without opening a backend.""" print(json.dumps(inspect_artifact(Path(cfg.artifact)), indent=2, sort_keys=True)) @@ -115,10 +191,28 @@ def _hydra_inspect_main(cfg: DictConfig) -> None: def main() -> None: """Adapt ``mxlab deploy`` arguments to the deployment Hydra commands.""" - if sys.argv[1:2] == ["inspect"]: - sys.argv = [f"{sys.argv[0]} inspect", *sys.argv[2:]] + args = sys.argv[1:] + if args[:1] == ["inspect"]: + sys.argv = [f"{sys.argv[0]} inspect", *args[1:]] _hydra_inspect_main() return - if not sys.argv[1:]: + + preset = next((argument.split("=", 1)[1] for argument in args if argument.startswith("deploy=")), None) + if preset is not None: + if preset not in _DEPLOY_PRESETS: + raise SystemExit(f"unknown deploy preset {preset!r}; choose from {sorted(_DEPLOY_PRESETS)}") + del sys.argv[1 + args.index(f"deploy={preset}")] + entry_point, implicit_arguments = _DEPLOY_PRESETS[preset] + sys.argv.extend(implicit_arguments) + entry_point() + return + + if any(argument == "program" or argument.startswith("program=") for argument in args): + if args[:1] == ["program"]: + del sys.argv[1] + _hydra_program_main() + return + + if not args: sys.argv.append("--help") _hydra_main() diff --git a/motrix_deploy/src/motrix_deploy/config/__init__.py b/motrix_deploy/src/motrix_deploy/config/__init__.py index c597c0e0..5bf867cb 100644 --- a/motrix_deploy/src/motrix_deploy/config/__init__.py +++ b/motrix_deploy/src/motrix_deploy/config/__init__.py @@ -95,6 +95,23 @@ def __post_init__(self) -> None: ) +@dataclass +class ProgramRunConfig: + """Resolved persistent multi-policy execution settings.""" + + program: Any = MISSING + runtime: Any = MISSING + + def __post_init__(self) -> None: + if self.program is MISSING or not getattr(self.program, "name", None): + raise ValueError("program must be a composed program recipe") + if not isinstance(self.runtime, (SimRuntimeCfg, HardwareRuntimeCfg)): + raise ValueError( + "runtime must be a composed SimRuntimeCfg or HardwareRuntimeCfg recipe, " + f"got {type(self.runtime).__name__}" + ) + + def _install_sim_runtime_schema() -> None: ConfigStore.instance().store(group=SIM_SCHEMA_GROUP, name="_sim_schema", node=SimRuntimeCfg) @@ -104,6 +121,7 @@ def _install_sim_runtime_schema() -> None: __all__ = [ "DeployRunConfig", "HardwareRuntimeCfg", + "ProgramRunConfig", "SIM_SCHEMA_GROUP", "SimRuntimeCfg", ] diff --git a/motrix_deploy/src/motrix_deploy/config/program-real.yaml b/motrix_deploy/src/motrix_deploy/config/program-real.yaml new file mode 100644 index 00000000..415feccb --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/config/program-real.yaml @@ -0,0 +1,24 @@ +# Hardware deployment application: same program and artifacts as the +# simulation application, but the Unitree SDK2 DDS runtime, starting in damping. +defaults: + - runtime: g1-unitree-state-machine + - program: g1-hot-swapping + - _self_ + - override hydra/job_logging: stdout + +hydra: + output_subdir: null + run: + dir: . + sweep: + dir: . + subdir: ${hydra.job.num} + job: + chdir: false + +program: + # Hardware always starts in damping and waits for the operator. + initial_state: passive + +runtime: + network_interface: eth0 diff --git a/motrix_deploy/src/motrix_deploy/config/program.yaml b/motrix_deploy/src/motrix_deploy/config/program.yaml new file mode 100644 index 00000000..18f5b4d1 --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/config/program.yaml @@ -0,0 +1,17 @@ +defaults: + - runtime: g1-sim-state-machine + - program: null + - _self_ + - override hydra/job_logging: stdout + +hydra: + output_subdir: null + run: + dir: . + sweep: + dir: . + subdir: ${hydra.job.num} + job: + chdir: false + +program: ??? diff --git a/motrix_deploy/src/motrix_deploy/errors.py b/motrix_deploy/src/motrix_deploy/errors.py index 74e10987..9791c588 100644 --- a/motrix_deploy/src/motrix_deploy/errors.py +++ b/motrix_deploy/src/motrix_deploy/errors.py @@ -26,6 +26,10 @@ class LieDownRequestedError(RuntimeError): """A physical input requested the backend's configured lie-down shutdown.""" +class PassiveRequestedError(RuntimeError): + """A physical input requested an immediate transition to passive damping.""" + + class ControlFailure(RuntimeError): def __init__(self, reason: str, message: str) -> None: self.reason = reason diff --git a/motrix_deploy/src/motrix_deploy/operator.py b/motrix_deploy/src/motrix_deploy/operator.py new file mode 100644 index 00000000..204e7dc0 --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/operator.py @@ -0,0 +1,284 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Backend-neutral operator gestures and command-input configuration.""" + +import time +from collections.abc import Sequence +from dataclasses import dataclass, field + +import numpy as np + +from motrix_deploy.errors import ValidationError +from motrix_env_core.input import CommandBinding, GamePadDevice, InputDevice, KeyboardDevice, PlanarVelocityCommand + + +@dataclass(frozen=True) +class OperatorCommand: + """Backend-neutral operator input consumed by a persistent control program.""" + + intents: frozenset[str] = frozenset() + planar_velocity: PlanarVelocityCommand | None = None + + def __post_init__(self) -> None: + if not isinstance(self.intents, frozenset) or any( + not isinstance(intent, str) or not intent for intent in self.intents + ): + raise ValidationError("operator.intents", "a frozenset of non-empty strings", self.intents) + if self.planar_velocity is not None and not isinstance(self.planar_velocity, PlanarVelocityCommand): + raise ValidationError( + "operator.planar_velocity", "a PlanarVelocityCommand or None", type(self.planar_velocity).__name__ + ) + + +@dataclass +class ButtonGestureCfg: + """One edge-triggered button gesture with optional held modifiers.""" + + held: list[str] = field(default_factory=list) + pressed: str | None = None + released: str | None = None + held_for_s: dict[str, float] = field(default_factory=dict) + + def __post_init__(self) -> None: + names = [*self.held, *self.held_for_s] + if self.pressed is not None: + names.append(self.pressed) + if self.released is not None: + names.append(self.released) + if not names: + raise ValidationError("operator.gesture", "at least one button condition", names) + if any(not isinstance(name, str) or not name for name in names): + raise ValidationError("operator.gesture.buttons", "non-empty button names", names) + if len(set(self.held)) != len(self.held): + raise ValidationError("operator.gesture.held", "unique button names", self.held) + for button, seconds in self.held_for_s.items(): + if ( + not isinstance(seconds, (int, float)) + or isinstance(seconds, bool) + or not np.isfinite(seconds) + or seconds <= 0 + ): + raise ValidationError(f"operator.gesture.held_for_s.{button}", "a positive finite duration", seconds) + + +@dataclass +class PlanarVelocityOperatorCfg: + """Map three normalized device axes into one planar velocity command.""" + + linear_x_axis: str = "ly" + linear_y_axis: str = "lx" + yaw_axis: str = "rx" + invert_linear_x: bool = False + invert_linear_y: bool = False + invert_yaw: bool = False + deadzone: float = 0.0 + range_scale: list[float] = field(default_factory=lambda: [1.0, 1.0, 1.0]) + deadman_button: str | None = None + + def __post_init__(self) -> None: + for name in (self.linear_x_axis, self.linear_y_axis, self.yaw_axis): + if not isinstance(name, str) or not name: + raise ValidationError("operator.planar_velocity.axes", "non-empty axis names", name) + if ( + not isinstance(self.deadzone, (int, float)) + or isinstance(self.deadzone, bool) + or not np.isfinite(self.deadzone) + or not 0 <= self.deadzone < 1 + ): + raise ValidationError("operator.planar_velocity.deadzone", "a finite value in [0, 1)", self.deadzone) + scale = np.asarray(self.range_scale, dtype=np.float32) + if scale.shape != (3,) or not np.all(np.isfinite(scale)) or np.any(scale < 0) or np.any(scale > 1): + raise ValidationError( + "operator.planar_velocity.range_scale", "three finite values in [0, 1]", self.range_scale + ) + if self.deadman_button is not None and (not isinstance(self.deadman_button, str) or not self.deadman_button): + raise ValidationError( + "operator.planar_velocity.deadman_button", "null or a non-empty button name", self.deadman_button + ) + + +@dataclass +class OperatorProfileCfg: + """User-configurable gestures plus optional continuous command wiring.""" + + intents: dict[str, ButtonGestureCfg] = field(default_factory=dict) + planar_velocity: PlanarVelocityOperatorCfg | None = None + + def __post_init__(self) -> None: + if any(not isinstance(name, str) or not name for name in self.intents): + raise ValidationError("operator.intents", "non-empty intent names", tuple(self.intents)) + signatures: dict[tuple[object, ...], str] = {} + for intent, gesture in self.intents.items(): + if not isinstance(gesture, ButtonGestureCfg): + raise ValidationError(f"operator.intents.{intent}", "a ButtonGestureCfg", type(gesture).__name__) + signature = ( + tuple(sorted(gesture.held)), + gesture.pressed, + gesture.released, + tuple(sorted(gesture.held_for_s.items())), + ) + previous = signatures.get(signature) + if previous is not None: + raise ValidationError( + "operator.intents", "distinct gestures for each intent", f"{previous!r} and {intent!r}" + ) + signatures[signature] = intent + + +class OperatorIntentEvaluator: + """Evaluate configured button gestures from timestamped button snapshots.""" + + def __init__(self, gestures: dict[str, ButtonGestureCfg]) -> None: + self._gestures = dict(gestures) + self._previous: frozenset[str] | None = None + self._pressed_since_ns: dict[str, int] = {} + + def reset(self) -> None: + self._previous = None + self._pressed_since_ns.clear() + + def update(self, pressed_buttons: frozenset[str], timestamp_ns: int) -> frozenset[str]: + """Return intents whose edge/hold predicates became true in this snapshot.""" + + if not isinstance(timestamp_ns, int) or isinstance(timestamp_ns, bool) or timestamp_ns < 0: + raise ValidationError("operator.timestamp_ns", "a non-negative integer", timestamp_ns) + previous = self._previous + for button in pressed_buttons: + self._pressed_since_ns.setdefault(button, timestamp_ns) + for button in tuple(self._pressed_since_ns): + if button not in pressed_buttons: + del self._pressed_since_ns[button] + self._previous = pressed_buttons + if previous is None: + return frozenset() + + matched = [] + for intent, gesture in self._gestures.items(): + if any(button not in pressed_buttons for button in gesture.held): + continue + if gesture.pressed is not None and not ( + gesture.pressed in pressed_buttons and gesture.pressed not in previous + ): + continue + if gesture.released is not None and not ( + gesture.released not in pressed_buttons and gesture.released in previous + ): + continue + if any( + button not in pressed_buttons + or timestamp_ns - self._pressed_since_ns.get(button, timestamp_ns) < round(seconds * 1e9) + for button, seconds in gesture.held_for_s.items() + ): + continue + matched.append(intent) + return frozenset(matched) + + +class OperatorCommandBinding(CommandBinding[OperatorCommand]): + """Read one input frame and produce semantic intents plus bounded velocity.""" + + def __init__( + self, + device: InputDevice, + profile: OperatorProfileCfg, + *, + command_lower: Sequence[float], + command_upper: Sequence[float], + ) -> None: + if not isinstance(device, (KeyboardDevice, GamePadDevice)): + raise ValidationError("operator.device", "a keyboard or gamepad", type(device).__name__) + self._device = device + self._profile = profile + self._evaluator = OperatorIntentEvaluator(profile.intents) + self._buttons = frozenset( + button + for gesture in profile.intents.values() + for button in (*gesture.held, *gesture.held_for_s, gesture.pressed, gesture.released) + if button is not None + ) + self._lower = np.asarray(command_lower, dtype=np.float32) + self._upper = np.asarray(command_upper, dtype=np.float32) + if self._lower.shape != (3,) or self._upper.shape != (3,): + raise ValidationError("operator.command_bounds", "two vectors with shape (3,)", (self._lower, self._upper)) + if np.any(self._lower > 0) or np.any(self._upper < 0) or np.any(self._lower > self._upper): + raise ValidationError( + "operator.command_bounds", "ordered bounds containing zero", (self._lower, self._upper) + ) + + def _pressed_buttons(self) -> frozenset[str]: + if isinstance(self._device, KeyboardDevice): + return frozenset(button for button in self._buttons if self._device.is_pressing(button)) + return frozenset(button for button in self._buttons if self._device.is_button_pressing(button)) + + def _keyboard_velocity(self) -> np.ndarray: + assert isinstance(self._device, KeyboardDevice) + direction = np.asarray( + ( + int(self._device.is_pressing("w")) - int(self._device.is_pressing("s")), + int(self._device.is_pressing("a")) - int(self._device.is_pressing("d")), + int(self._device.is_pressing("q")) - int(self._device.is_pressing("e")), + ), + dtype=np.int8, + ) + return np.where(direction > 0, self._upper, np.where(direction < 0, self._lower, 0)).astype(np.float32) + + def _gamepad_velocity(self) -> np.ndarray: + assert isinstance(self._device, GamePadDevice) + config = self._profile.planar_velocity + if config is None: + return np.zeros(3, dtype=np.float32) + if config.deadman_button is not None and not self._device.is_button_pressing(config.deadman_button): + return np.zeros(3, dtype=np.float32) + axes = np.asarray( + ( + self._device.axis_value(config.linear_x_axis), + self._device.axis_value(config.linear_y_axis), + self._device.axis_value(config.yaw_axis), + ), + dtype=np.float32, + ) + direction = np.asarray( + ( + -1.0 if config.invert_linear_x else 1.0, + -1.0 if config.invert_linear_y else 1.0, + -1.0 if config.invert_yaw else 1.0, + ), + dtype=np.float32, + ) + axes = np.clip(axes * direction, -1.0, 1.0) + magnitude = np.maximum(np.abs(axes) - config.deadzone, 0.0) / (1.0 - config.deadzone) + normalized = np.copysign(magnitude, axes) + bounded = np.where(normalized >= 0, normalized * self._upper, (-normalized) * self._lower) + return (bounded * np.asarray(config.range_scale, dtype=np.float32)).astype(np.float32) + + def read_command(self, *, batch_size: int = 1) -> OperatorCommand: + if not isinstance(batch_size, int) or isinstance(batch_size, bool) or batch_size <= 0: + raise ValidationError("operator.batch_size", "a positive integer", batch_size) + self._device.poll() + intents = self._evaluator.update(self._pressed_buttons(), time.monotonic_ns()) + velocity = self._keyboard_velocity() if isinstance(self._device, KeyboardDevice) else self._gamepad_velocity() + return OperatorCommand( + intents=intents, + planar_velocity=PlanarVelocityCommand(np.repeat(velocity[None, :], batch_size, axis=0)), + ) + + +class ConstantOperatorCommandBinding(CommandBinding[OperatorCommand]): + """Keep a headless program passive until an external binding is supplied.""" + + def read_command(self, *, batch_size: int = 1) -> OperatorCommand: + if not isinstance(batch_size, int) or isinstance(batch_size, bool) or batch_size <= 0: + raise ValidationError("operator.batch_size", "a positive integer", batch_size) + return OperatorCommand(planar_velocity=PlanarVelocityCommand(np.zeros((batch_size, 3), dtype=np.float32))) + + +__all__ = [ + "ButtonGestureCfg", + "ConstantOperatorCommandBinding", + "OperatorCommand", + "OperatorCommandBinding", + "OperatorIntentEvaluator", + "OperatorProfileCfg", + "PlanarVelocityOperatorCfg", +] diff --git a/motrix_deploy/src/motrix_deploy/program.py b/motrix_deploy/src/motrix_deploy/program.py new file mode 100644 index 00000000..7d352999 --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/program.py @@ -0,0 +1,63 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Persistent multi-policy deployment program discovery and assembly.""" + +from collections.abc import Callable +from dataclasses import dataclass +from importlib import metadata +from typing import Any + +from motrix_deploy.artifact.schema import ControlSpec +from motrix_deploy.contracts import FloatArray, RobotSpec +from motrix_deploy.operator import ( + ConstantOperatorCommandBinding, + OperatorCommand, + OperatorCommandBinding, + OperatorProfileCfg, +) +from motrix_deploy.runtime.lifecycle import Controller +from motrix_env_core.input import CommandBinding, InputDevice + +PROGRAM_ENTRY_POINT_GROUP = "motrix_deploy.programs" + + +@dataclass(frozen=True) +class DeploymentProgram: + """Fully loaded persistent controller plus its backend-neutral contracts.""" + + robot: RobotSpec + control: ControlSpec + controller: Controller[OperatorCommand] + command_lower: FloatArray + command_upper: FloatArray + operator: OperatorProfileCfg + + def prepare(self, device: InputDevice | None) -> CommandBinding[OperatorCommand]: + if device is None: + return ConstantOperatorCommandBinding() + return OperatorCommandBinding( + device, + self.operator, + command_lower=self.command_lower, + command_upper=self.command_upper, + ) + + +def create_program(config: Any) -> DeploymentProgram: + """Load a program factory by the typed config's stable name.""" + name = getattr(config, "name", None) + if not isinstance(name, str) or not name: + raise ValueError("program config must provide a non-empty name") + matches = [entry for entry in metadata.entry_points(group=PROGRAM_ENTRY_POINT_GROUP) if entry.name == name] + if len(matches) != 1: + available = sorted(entry.name for entry in metadata.entry_points(group=PROGRAM_ENTRY_POINT_GROUP)) + raise ValueError(f"Deployment program {name!r} has {len(matches)} providers; available: {available}") + factory: Callable[[Any], DeploymentProgram] = matches[0].load() + program = factory(config) + if not isinstance(program, DeploymentProgram): + raise TypeError(f"Deployment program {name!r} returned {type(program).__name__}") + return program + + +__all__ = ["DeploymentProgram", "create_program"] diff --git a/motrix_deploy/src/motrix_deploy/robot/g1_standing.py b/motrix_deploy/src/motrix_deploy/robot/g1_standing.py new file mode 100644 index 00000000..868cabb1 --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/robot/g1_standing.py @@ -0,0 +1,102 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Fixed G1 29-DOF standing (FixStand) servo gains. + +FixStand is a posture, not a policy: it must not follow the exported policy +artifact, otherwise every new policy silently changes how the robot stands up. +The persistent program, the skill preparation and the single-artifact hardware +startup all resolve their pull-in gains here, so there is exactly one definition +to adjust by hand. + +Values are keyed by joint name so any canonical joint order resolves correctly. +They are the field-validated stand-up gains: stiffer than the policy gains so the +legs carry the body, with waist gains that hold the torso and light arm damping. +""" + +from __future__ import annotations + +from collections.abc import Mapping, Sequence + +import numpy as np + +G1_29DOF_STANDING_KP: Mapping[str, float] = { + "left_hip_pitch_joint": 350.0, + "left_hip_roll_joint": 200.0, + "left_hip_yaw_joint": 200.0, + "left_knee_joint": 300.0, + "left_ankle_pitch_joint": 300.0, + "left_ankle_roll_joint": 150.0, + "right_hip_pitch_joint": 350.0, + "right_hip_roll_joint": 200.0, + "right_hip_yaw_joint": 200.0, + "right_knee_joint": 300.0, + "right_ankle_pitch_joint": 300.0, + "right_ankle_roll_joint": 150.0, + "waist_yaw_joint": 200.0, + "waist_roll_joint": 200.0, + "waist_pitch_joint": 200.0, + "left_shoulder_pitch_joint": 40.0, + "left_shoulder_roll_joint": 40.0, + "left_shoulder_yaw_joint": 40.0, + "left_elbow_joint": 40.0, + "left_wrist_roll_joint": 40.0, + "left_wrist_pitch_joint": 40.0, + "left_wrist_yaw_joint": 40.0, + "right_shoulder_pitch_joint": 40.0, + "right_shoulder_roll_joint": 40.0, + "right_shoulder_yaw_joint": 40.0, + "right_elbow_joint": 40.0, + "right_wrist_roll_joint": 40.0, + "right_wrist_pitch_joint": 40.0, + "right_wrist_yaw_joint": 40.0, +} + +G1_29DOF_STANDING_KD: Mapping[str, float] = { + "left_hip_pitch_joint": 5.0, + "left_hip_roll_joint": 5.0, + "left_hip_yaw_joint": 5.0, + "left_knee_joint": 10.0, + "left_ankle_pitch_joint": 5.0, + "left_ankle_roll_joint": 5.0, + "right_hip_pitch_joint": 5.0, + "right_hip_roll_joint": 5.0, + "right_hip_yaw_joint": 5.0, + "right_knee_joint": 10.0, + "right_ankle_pitch_joint": 5.0, + "right_ankle_roll_joint": 5.0, + "waist_yaw_joint": 5.0, + "waist_roll_joint": 5.0, + "waist_pitch_joint": 5.0, + "left_shoulder_pitch_joint": 3.0, + "left_shoulder_roll_joint": 3.0, + "left_shoulder_yaw_joint": 3.0, + "left_elbow_joint": 3.0, + "left_wrist_roll_joint": 3.0, + "left_wrist_pitch_joint": 3.0, + "left_wrist_yaw_joint": 3.0, + "right_shoulder_pitch_joint": 3.0, + "right_shoulder_roll_joint": 3.0, + "right_shoulder_yaw_joint": 3.0, + "right_elbow_joint": 3.0, + "right_wrist_roll_joint": 3.0, + "right_wrist_pitch_joint": 3.0, + "right_wrist_yaw_joint": 3.0, +} + + +def resolve_g1_standing_gains(joint_names: Sequence[str]) -> tuple[np.ndarray, np.ndarray]: + """Return the fixed standing gains in the requested canonical joint order.""" + names = tuple(joint_names) + known = set(G1_29DOF_STANDING_KP) + if set(names) != known: + raise ValueError( + "G1 standing gains cover exactly the 29 G1 DOF joints; " + f"missing={sorted(known - set(names))}, unknown={sorted(set(names) - known)}" + ) + kp = np.asarray([G1_29DOF_STANDING_KP[name] for name in names], dtype=np.float32) + kd = np.asarray([G1_29DOF_STANDING_KD[name] for name in names], dtype=np.float32) + return kp, kd + + +__all__ = ["G1_29DOF_STANDING_KD", "G1_29DOF_STANDING_KP", "resolve_g1_standing_gains"] diff --git a/motrix_deploy/src/motrix_deploy/runtime/control.py b/motrix_deploy/src/motrix_deploy/runtime/control.py index e2148e06..724c92d3 100644 --- a/motrix_deploy/src/motrix_deploy/runtime/control.py +++ b/motrix_deploy/src/motrix_deploy/runtime/control.py @@ -8,7 +8,13 @@ from typing import Generic, TypeVar from motrix_deploy.contracts import JointServoCommand, RobotState -from motrix_deploy.errors import ControlFailure, EmergencyStopError, LieDownRequestedError, ValidationError +from motrix_deploy.errors import ( + ControlFailure, + EmergencyStopError, + LieDownRequestedError, + PassiveRequestedError, + ValidationError, +) from motrix_deploy.policy import Policy from motrix_deploy.policy.processing import PolicyProcessor from motrix_deploy.robot.interface import RobotInterface @@ -164,7 +170,7 @@ def start(self) -> bool: if self.robot.capabilities.requires_enable: try: self.robot.enable() - except (EmergencyStopError, LieDownRequestedError, ValidationError): + except (EmergencyStopError, LieDownRequestedError, PassiveRequestedError, ValidationError): raise except Exception as error: raise ControlFailure("backend_enable_error", str(error)) from error @@ -181,7 +187,7 @@ def _read_state(self, previous: int | None) -> "RobotState": state = self.robot.read_state(self.state_timeout_s) except TimeoutError as error: raise ControlFailure("state_timeout", str(error)) from error - except (EmergencyStopError, LieDownRequestedError): + except (EmergencyStopError, LieDownRequestedError, PassiveRequestedError): raise except Exception as error: raise ControlFailure("backend_read_error", str(error)) from error @@ -236,7 +242,7 @@ def tick(self, *, elapsed_time_s: float) -> bool: phase = time.monotonic_ns() try: self.robot.write_command(decision.command) - except (EmergencyStopError, LieDownRequestedError): + except (EmergencyStopError, LieDownRequestedError, PassiveRequestedError): raise except Exception as error: raise ControlFailure("backend_write_error", str(error)) from error @@ -267,6 +273,8 @@ def fail(self, error: BaseException) -> None: self._reason, self._message = "emergency_stop", str(error) elif isinstance(error, LieDownRequestedError): self._reason, self._message = "lie_down", str(error) + elif isinstance(error, PassiveRequestedError): + self._reason, self._message = "passive", str(error) elif isinstance(error, ControlFailure): self._reason, self._message = error.reason, str(error) elif isinstance(error, ValidationError): diff --git a/motrix_deploy/src/motrix_deploy/runtime/hot_swapping.py b/motrix_deploy/src/motrix_deploy/runtime/hot_swapping.py new file mode 100644 index 00000000..899b21cc --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/runtime/hot_swapping.py @@ -0,0 +1,147 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Backend-neutral orchestration for baseline states and finite skill modules.""" + +from __future__ import annotations + +from dataclasses import dataclass + +from motrix_deploy.contracts import RobotState +from motrix_deploy.errors import ValidationError +from motrix_deploy.operator import OperatorCommand +from motrix_deploy.runtime.preparation import JointServoPreparationState, SkillPreparationSpec +from motrix_deploy.runtime.state_machine import ( + ControlState, + ImmediateTransition, + StateMachineController, + TransitionFactory, + TransitionRule, +) + + +@dataclass(frozen=True) +class BaselineStates: + """Persistent passive, fixed-standing and locomotion states.""" + + passive: ControlState + fixstand: ControlState + walk: ControlState + + +@dataclass(frozen=True) +class SkillModule: + """One finite policy state and the preparation required before entering it.""" + + name: str + state: ControlState + preparation: SkillPreparationSpec + request_intent: str + + def __post_init__(self) -> None: + if self.name != self.state.name: + raise ValueError("skill module name must equal its control state name") + + +class RobotHotSwappingController(StateMachineController): + """Run P/A/B/C/D with one shared preparation state for all skills.""" + + def __init__( + self, + *, + joint_names: tuple[str, ...], + baseline: BaselineStates, + preparation: JointServoPreparationState, + skills: tuple[SkillModule, ...], + initial_state: str, + baseline_transitions: tuple[TransitionRule, ...], + passive_transition: TransitionFactory, + emergency_intents: tuple[str, ...] = ("request_passive", "request_emergency"), + ) -> None: + if not skills: + raise ValidationError("hot_swapping.skills", "at least one skill module", skills) + self._preparation = preparation + self._skills = {module.name: module for module in skills} + if len(self._skills) != len(skills): + raise ValidationError("hot_swapping.skills", "unique skill names", tuple(self._skills)) + self._skill_intents = {module.request_intent: module for module in skills} + if len(self._skill_intents) != len(skills): + raise ValidationError("hot_swapping.skills", "unique request intents", tuple(self._skill_intents)) + self._pending_skill: str | None = None + + states = ( + baseline.passive, + baseline.fixstand, + baseline.walk, + preparation, + *(module.state for module in skills), + ) + passive_name = baseline.passive.name + transitions = ( + *baseline_transitions, + TransitionRule(baseline.walk.name, preparation.name, ImmediateTransition), + *(TransitionRule(preparation.name, module.name, ImmediateTransition) for module in skills), + *(TransitionRule(module.name, baseline.walk.name, ImmediateTransition) for module in skills), + *( + TransitionRule(state.name, passive_name, passive_transition) + for state in states + if state.name != passive_name + and not any(rule.source == state.name and rule.target == passive_name for rule in baseline_transitions) + ), + ) + self._walk_name = baseline.walk.name + super().__init__( + joint_names=joint_names, + states={state.name: state for state in states}, + initial_state=initial_state, + transitions=transitions, + intent_targets={ + "request_fixstand": baseline.fixstand.name, + "request_walk": baseline.walk.name, + }, + emergency_intent=emergency_intents, + emergency_state=passive_name, + ) + + @property + def pending_skill(self) -> str | None: + return self._pending_skill + + def reset(self, state: RobotState) -> None: + self._pending_skill = None + super().reset(state) + + def _target_from_intents(self, command: OperatorCommand) -> str | None: + target = super()._target_from_intents(command) + if target == self._emergency_state: + self._pending_skill = None + return target + protected = {self._preparation.name, *self._skills} + if self._current in protected or (self._active is not None and self._active.target in protected): + return None + requested_modules = [self._skill_intents[intent] for intent in command.intents if intent in self._skill_intents] + if len(requested_modules) > 1 or (target is not None and requested_modules): + raise ValidationError("operator.intents", "at most one transition request", command.intents) + if not requested_modules: + return target + # Skills may only be armed from stable locomotion. Requests received in + # P/A/C/D or during a transition are deliberately ignored. + if self._current != self._walk_name or self._active is not None: + return None + module = requested_modules[0] + self._pending_skill = module.name + self._preparation.select(module.preparation) + return self._preparation.name + + def _completion_target(self, state_name: str) -> str | None: + if state_name == self._preparation.name: + if self._pending_skill is None: + raise RuntimeError("preparation completed without a pending skill") + return self._pending_skill + if state_name in self._skills: + self._pending_skill = None + return self._walk_name + return super()._completion_target(state_name) + + +__all__ = ["BaselineStates", "RobotHotSwappingController", "SkillModule"] diff --git a/motrix_deploy/src/motrix_deploy/runtime/preparation.py b/motrix_deploy/src/motrix_deploy/runtime/preparation.py new file mode 100644 index 00000000..dbeb9ebf --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/runtime/preparation.py @@ -0,0 +1,142 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Reusable measured-state preparation before a finite skill takes control.""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + +from motrix_deploy.contracts import FloatArray, JointServoCommand, RobotSpec, RobotState, float32_array +from motrix_deploy.operator import OperatorCommand +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy.runtime.lifecycle import ControllerStep + + +@dataclass(frozen=True) +class SkillPreparationSpec: + """Servo target and measured readiness requirements for one skill module.""" + + joint_position: FloatArray + kp: FloatArray + kd: FloatArray + 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 + + def __post_init__(self) -> None: + nonnegative = ( + "ramp_duration_s", + "settle_duration_s", + "max_joint_error_rad", + "max_joint_velocity_rad_s", + "max_base_tilt_rad", + ) + for name in nonnegative: + value = getattr(self, name) + if not np.isfinite(value) or value < 0: + raise ValueError(f"{name} must be finite and non-negative") + if not np.isfinite(self.timeout_s) or self.timeout_s <= 0: + raise ValueError("timeout_s must be positive and finite") + + +class JointServoPreparationState: + """Prepare whichever skill is pending with one reusable top-level state.""" + + name = "skill_preparation" + + def __init__(self, robot: RobotSpec) -> None: + self.joint_names = robot.joint_names + self._robot = robot + self._spec: SkillPreparationSpec | None = None + self._target = np.empty(robot.joint_count, dtype=np.float32) + self._kp = np.empty(robot.joint_count, dtype=np.float32) + self._kd = np.empty(robot.joint_count, dtype=np.float32) + self._zeros = np.zeros(robot.joint_count, dtype=np.float32) + self._initial_position: FloatArray | None = None + self._ready_since: float | None = None + self._reached_ready = False + self._stop_requested = False + + @property + def spec(self) -> SkillPreparationSpec | None: + return self._spec + + def select(self, spec: SkillPreparationSpec) -> None: + """Select the preparation contract for the next requested skill.""" + joint_count = self._robot.joint_count + self._target = np.clip( + float32_array(spec.joint_position, path="skill.preparation.joint_position", shape=(joint_count,)), + self._robot.position_lower, + self._robot.position_upper, + ).astype(np.float32) + self._kp = float32_array(spec.kp, path="skill.preparation.kp", shape=(joint_count,)) + self._kd = float32_array(spec.kd, path="skill.preparation.kd", shape=(joint_count,)) + self._spec = spec + + def reset(self, state: RobotState) -> None: + if self._spec is None: + raise RuntimeError("a skill preparation spec must be selected before entering preparation") + self._initial_position = state.joint_position.copy() + self._ready_since = None + self._reached_ready = False + self._stop_requested = False + + def request_stop(self) -> None: + self._stop_requested = True + + def hold_command(self) -> JointServoCommand: + """Hold the selected target after readiness has been reached.""" + return JointServoCommand(self._target, self._zeros, self._zeros, self._kp, self._kd) + + def step(self, state: RobotState, context: ControlContext[OperatorCommand]) -> ControllerStep: + if self._stop_requested: + return ControllerStep(complete=True) + assert self._spec is not None and self._initial_position is not None + spec = self._spec + elapsed_time_s = context.elapsed_time_s + 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 > spec.max_base_tilt_rad: + return ControllerStep(metrics=metrics, error=f"unsafe base tilt during preparation: {metrics}") + ready = ( + elapsed_time_s + 1e-9 >= spec.ramp_duration_s + and metrics["max_joint_error_rad"] <= spec.max_joint_error_rad + and metrics["max_joint_velocity_rad_s"] <= spec.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 >= spec.settle_duration_s + and (self._reached_ready or elapsed_time_s <= spec.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 >= spec.timeout_s: + return ControllerStep(metrics=metrics, error=f"preparation timed out: {metrics}") + fraction = ( + np.clip((elapsed_time_s + context.dt_s) / spec.ramp_duration_s, 0, 1) if spec.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, + ) + + +__all__ = ["JointServoPreparationState", "SkillPreparationSpec"] diff --git a/motrix_deploy/src/motrix_deploy/runtime/state_machine.py b/motrix_deploy/src/motrix_deploy/runtime/state_machine.py new file mode 100644 index 00000000..9464002c --- /dev/null +++ b/motrix_deploy/src/motrix_deploy/runtime/state_machine.py @@ -0,0 +1,383 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Backend-neutral persistent control states and command transitions.""" + +from __future__ import annotations + +from collections import deque +from collections.abc import Callable, Mapping +from dataclasses import dataclass, replace +from typing import Protocol + +import numpy as np + +from motrix_deploy.contracts import JointServoCommand, JointTorqueCommand, RobotCommand, RobotState +from motrix_deploy.errors import ValidationError +from motrix_deploy.operator import OperatorCommand +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy.runtime.lifecycle import ControllerStep + + +class ControlState(Protocol): + """One persistent state that produces commands in its declared joint order.""" + + name: str + joint_names: tuple[str, ...] + + def reset(self, state: RobotState) -> None: ... + + def step(self, state: RobotState, context: ControlContext[OperatorCommand]) -> ControllerStep: ... + + def request_stop(self) -> None: ... + + +class CommandTransition(Protocol): + """Stateful blend from a frozen source command to live target commands.""" + + def start(self, source: RobotCommand, target: RobotCommand) -> None: ... + + def step(self, target: RobotCommand, elapsed_time_s: float) -> tuple[RobotCommand, bool]: ... + + +TransitionFactory = Callable[[], CommandTransition] + + +@dataclass(frozen=True) +class TransitionRule: + """One legal directed state edge and the command blend used on that edge.""" + + source: str + target: str + create: TransitionFactory + + +def _quintic_smoothstep(value: float) -> np.float32: + s = np.float32(np.clip(value, 0.0, 1.0)) + return np.float32(10.0 * s**3 - 15.0 * s**4 + 6.0 * s**5) + + +def _blend_array(source: np.ndarray, target: np.ndarray, alpha: np.float32) -> np.ndarray: + return (source + alpha * (target - source)).astype(np.float32) + + +class SmoothServoTransition: + """Blend position, velocity, feed-forward torque and gains with zero endpoint slope.""" + + def __init__(self, duration_s: float) -> None: + if not np.isfinite(duration_s) or duration_s <= 0: + raise ValidationError("transition.duration_s", "a positive finite value", duration_s) + self.duration_s = float(duration_s) + self._source: JointServoCommand | None = None + + def start(self, source: RobotCommand, target: RobotCommand) -> None: + if not isinstance(source, JointServoCommand) or not isinstance(target, JointServoCommand): + raise ValidationError("transition.command", "joint-servo source and target commands", (source, target)) + if source.joint_position.shape != target.joint_position.shape: + raise ValidationError( + "transition.command.shape", str(source.joint_position.shape), target.joint_position.shape + ) + self._source = source + + def step(self, target: RobotCommand, elapsed_time_s: float) -> tuple[RobotCommand, bool]: + if self._source is None: + raise RuntimeError("transition must be started before stepping") + if not isinstance(target, JointServoCommand): + raise ValidationError("transition.target", "a joint-servo command", type(target).__name__) + alpha = _quintic_smoothstep(elapsed_time_s / self.duration_s) + source = self._source + command = JointServoCommand( + joint_position=_blend_array(source.joint_position, target.joint_position, alpha), + joint_velocity=_blend_array(source.joint_velocity, target.joint_velocity, alpha), + feedforward_torque=_blend_array(source.feedforward_torque, target.feedforward_torque, alpha), + kp=_blend_array(source.kp, target.kp, alpha), + kd=_blend_array(source.kd, target.kd, alpha), + ) + return command, elapsed_time_s + 1e-9 >= self.duration_s + + +class AdaptiveServoTransition(SmoothServoTransition): + """Choose duration from the largest commanded joint displacement.""" + + def __init__(self, min_duration_s: float, max_duration_s: float, max_joint_speed_rad_s: float) -> None: + if not 0 < min_duration_s <= max_duration_s: + raise ValidationError( + "transition.duration_range", "0 < min_duration_s <= max_duration_s", (min_duration_s, max_duration_s) + ) + if not np.isfinite(max_joint_speed_rad_s) or max_joint_speed_rad_s <= 0: + raise ValidationError("transition.max_joint_speed_rad_s", "a positive finite value", max_joint_speed_rad_s) + self.min_duration_s = float(min_duration_s) + self.max_duration_s = float(max_duration_s) + self.max_joint_speed_rad_s = float(max_joint_speed_rad_s) + super().__init__(min_duration_s) + + def start(self, source: RobotCommand, target: RobotCommand) -> None: + super().start(source, target) + assert isinstance(source, JointServoCommand) and isinstance(target, JointServoCommand) + displacement = float(np.max(np.abs(target.joint_position - source.joint_position))) + self.duration_s = float( + np.clip(displacement / self.max_joint_speed_rad_s, self.min_duration_s, self.max_duration_s) + ) + + +class ImmediateTransition: + """Select the target command without blending, for emergency paths.""" + + def start(self, source: RobotCommand, target: RobotCommand) -> None: + del source, target + + def step(self, target: RobotCommand, elapsed_time_s: float) -> tuple[RobotCommand, bool]: + del elapsed_time_s + return target, True + + +@dataclass +class _ActiveTransition: + target: str + transition: CommandTransition + origin_s: float + + +class StateMachineController: + """Run persistent states while keeping backend command order and lifecycle stable.""" + + def __init__( + self, + *, + joint_names: tuple[str, ...], + states: Mapping[str, ControlState], + initial_state: str, + transitions: tuple[TransitionRule, ...], + intent_targets: Mapping[str, str], + completion_targets: Mapping[str, str] | None = None, + emergency_intent: str | tuple[str, ...] = "request_emergency", + emergency_state: str = "emergency_damping", + ) -> None: + if not joint_names or len(joint_names) != len(set(joint_names)): + raise ValidationError("state_machine.joint_names", "unique joint names", joint_names) + self.joint_names = joint_names + self.states = dict(states) + if set(self.states) != {state.name for state in self.states.values()}: + raise ValidationError("state_machine.states", "mapping keys equal state names", tuple(self.states)) + if initial_state not in self.states: + raise ValidationError("state_machine.initial_state", "a configured state", initial_state) + for state in self.states.values(): + if set(state.joint_names) != set(joint_names): + raise ValidationError( + f"state_machine.states.{state.name}.joint_names", "the canonical joint-name set", state.joint_names + ) + self._rules = {(rule.source, rule.target): rule.create for rule in transitions} + if len(self._rules) != len(transitions): + raise ValidationError("state_machine.transitions", "unique directed edges", transitions) + for source, target in self._rules: + if source not in self.states or target not in self.states: + raise ValidationError("state_machine.transitions", "edges between configured states", (source, target)) + self._intent_targets = dict(intent_targets) + self._completion_targets = dict(completion_targets or {}) + for target in (*self._intent_targets.values(), *self._completion_targets.values(), emergency_state): + if target not in self.states: + raise ValidationError("state_machine.target", "a configured state", target) + self._initial_state = initial_state + self._current = initial_state + self._emergency_intents = ( + frozenset({emergency_intent}) if isinstance(emergency_intent, str) else frozenset(emergency_intent) + ) + self._emergency_state = emergency_state + self._active: _ActiveTransition | None = None + self._queued_targets: list[str] = [] + self._last_command: RobotCommand | None = None + self._state_origin_s = 0.0 + self._state_step = 0 + self._stop_requested = False + + @property + def current_state(self) -> str: + return self._current + + @property + def target_state(self) -> str | None: + return None if self._active is None else self._active.target + + def reset(self, state: RobotState) -> None: + self._current = self._initial_state + self._active = None + self._queued_targets.clear() + self._last_command = None + self._state_origin_s = 0.0 + self._state_step = 0 + self._stop_requested = False + self.states[self._current].reset(state) + + def request_stop(self) -> None: + self._stop_requested = True + + def _state_context( + self, context: ControlContext[OperatorCommand], command: OperatorCommand | None = None + ) -> ControlContext[OperatorCommand]: + return ControlContext( + step=self._state_step, + elapsed_time_s=max(0.0, context.elapsed_time_s - self._state_origin_s), + command=context.command if command is None else command, + dt_s=context.dt_s, + ) + + def _canonical_command(self, command: RobotCommand, state_name: str) -> RobotCommand: + source_names = self.states[state_name].joint_names + indices = np.asarray([source_names.index(name) for name in self.joint_names], dtype=np.int64) + if isinstance(command, JointServoCommand): + return JointServoCommand( + command.joint_position[indices].astype(np.float32), + command.joint_velocity[indices].astype(np.float32), + command.feedforward_torque[indices].astype(np.float32), + command.kp[indices].astype(np.float32), + command.kd[indices].astype(np.float32), + ) + if isinstance(command, JointTorqueCommand): + return JointTorqueCommand(command.torque[indices].astype(np.float32)) + raise ValidationError("state_machine.command", "a RobotCommand", type(command).__name__) + + def _target_from_intents(self, command: OperatorCommand) -> str | None: + if self._emergency_intents.intersection(command.intents): + return self._emergency_state + targets = {self._intent_targets[intent] for intent in command.intents if intent in self._intent_targets} + if len(targets) > 1: + raise ValidationError("operator.intents", "at most one state transition request", command.intents) + return next(iter(targets), None) + + def _completion_target(self, state_name: str) -> str | None: + return self._completion_targets.get(state_name) + + def _path_to(self, source: str, target: str) -> list[str]: + """Return the shortest configured route, preserving declaration order.""" + if source == target: + return [] + frontier: deque[tuple[str, list[str]]] = deque([(source, [])]) + visited = {source} + while frontier: + node, path = frontier.popleft() + for edge_source, edge_target in self._rules: + if edge_source != node or edge_target in visited: + continue + next_path = [*path, edge_target] + if edge_target == target: + return next_path + visited.add(edge_target) + frontier.append((edge_target, next_path)) + raise ValidationError("state_machine.transition", "a configured route", (source, target)) + + def _begin_routed_transition( + self, + target: str, + state: RobotState, + context: ControlContext[OperatorCommand], + source_command: RobotCommand, + ) -> ControllerStep: + path = self._path_to(self._current, target) + if not path: + return ControllerStep(command=source_command) + self._queued_targets = path[1:] + return self._begin_transition(path[0], state, context, source_command) + + def _begin_transition( + self, + target: str, + state: RobotState, + context: ControlContext[OperatorCommand], + source_command: RobotCommand, + ) -> ControllerStep: + if target == self._current: + return ControllerStep(command=source_command) + factory = self._rules.get((self._current, target)) + if factory is None: + raise ValidationError("state_machine.transition", "a configured legal edge", (self._current, target)) + target_state = self.states[target] + target_state.reset(state) + self._state_origin_s = context.elapsed_time_s + self._state_step = 0 + target_step = target_state.step(state, self._state_context(context)) + if target_step.error is not None: + return target_step + if target_step.command is None: + if not target_step.complete: + return target_step + self._current = target + completion_target = self._completion_target(target) + if completion_target is None: + return target_step + return self._begin_transition(completion_target, state, context, source_command) + target_command = self._canonical_command(target_step.command, target) + transition = factory() + transition.start(source_command, target_command) + self._active = _ActiveTransition(target=target, transition=transition, origin_s=context.elapsed_time_s) + blended, complete = transition.step(target_command, 0.0) + if complete: + self._current = target + self._active = None + self._state_step += 1 + return replace(target_step, command=blended, complete=False) + + def step(self, state: RobotState, context: ControlContext[OperatorCommand]) -> ControllerStep: + if self._stop_requested: + return ControllerStep(complete=True) + operator = context.command + if not isinstance(operator, OperatorCommand): + raise ValidationError("state_machine.command", "an OperatorCommand", type(operator).__name__) + requested = self._target_from_intents(operator) + if self._active is not None: + if requested == self._emergency_state and self._active.target != self._emergency_state: + assert self._last_command is not None + self._current = self._active.target + self._active = None + self._queued_targets.clear() + return self._begin_routed_transition(requested, state, context, self._last_command) + target = self._active.target + decision = self.states[target].step(state, self._state_context(context)) + if decision.error is not None or decision.command is None: + return decision + target_command = self._canonical_command(decision.command, target) + output, complete = self._active.transition.step( + target_command, context.elapsed_time_s - self._active.origin_s + ) + self._last_command = output + self._state_step += 1 + if complete: + self._current = target + self._active = None + return replace(decision, command=output, complete=False) + + decision = self.states[self._current].step(state, self._state_context(context)) + if decision.error is not None: + return decision + if decision.command is None: + if decision.complete: + target = requested if requested is not None else self._completion_target(self._current) + if target is None: + return decision + if self._last_command is None: + raise ValidationError("state_machine.command", "a command before state completion", None) + return self._begin_transition(target, state, context, self._last_command) + raise ValidationError("state_machine.command", "one command per active interval", None) + command = self._canonical_command(decision.command, self._current) + self._last_command = command + self._state_step += 1 + target = requested + if target == self._emergency_state: + self._queued_targets.clear() + elif target is None and self._queued_targets: + target = self._queued_targets.pop(0) + if target is None and decision.complete: + target = self._completion_target(self._current) + if target is not None: + return self._begin_routed_transition(target, state, context, command) + return replace(decision, command=command, complete=False) + + +__all__ = [ + "AdaptiveServoTransition", + "CommandTransition", + "ControlState", + "ImmediateTransition", + "SmoothServoTransition", + "StateMachineController", + "TransitionRule", +] diff --git a/motrix_deploy/src/motrix_deploy/task.py b/motrix_deploy/src/motrix_deploy/task.py index afb37ead..37cd5ed7 100644 --- a/motrix_deploy/src/motrix_deploy/task.py +++ b/motrix_deploy/src/motrix_deploy/task.py @@ -9,6 +9,7 @@ from motrix_deploy.artifact.schema import TaskSpec from motrix_deploy.contracts import FloatArray, RobotSpec, RobotState +from motrix_deploy.operator import OperatorProfileCfg from motrix_deploy.policy import Policy from motrix_deploy.runtime.context import ControlContext from motrix_deploy.runtime.lifecycle import ControllerStep @@ -62,6 +63,11 @@ def prepare( self.validate_command(None) return None + def configure_operator(self, profile: OperatorProfileCfg | None) -> None: + """Accept task-owned command wiring before ``prepare`` assembles its binding.""" + if profile is not None and profile.planar_velocity is not None: + raise ValueError(f"{type(self).__name__} does not support planar velocity operator input") + @property def operator_hint(self) -> str | None: """One-line operator key reference, or None when no operator input is attached.""" diff --git a/motrix_deploy/tests/test_hot_swapping.py b/motrix_deploy/tests/test_hot_swapping.py new file mode 100644 index 00000000..903221d1 --- /dev/null +++ b/motrix_deploy/tests/test_hot_swapping.py @@ -0,0 +1,151 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +from dataclasses import dataclass + +import numpy as np + +from motrix_deploy.contracts import JointServoCommand, RobotSpec, RobotState +from motrix_deploy.operator import OperatorCommand +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy.runtime.hot_swapping import BaselineStates, RobotHotSwappingController, SkillModule +from motrix_deploy.runtime.lifecycle import ControllerStep +from motrix_deploy.runtime.preparation import JointServoPreparationState, SkillPreparationSpec +from motrix_deploy.runtime.state_machine import ImmediateTransition, TransitionRule + + +@dataclass +class _State: + name: str + joint_names: tuple[str, ...] + target: float + complete_after: int | None = None + + def reset(self, state: RobotState) -> None: + del state + self.steps = 0 + + def request_stop(self) -> None: + pass + + def step(self, state: RobotState, context: ControlContext[OperatorCommand]) -> ControllerStep: + del state, context + self.steps += 1 + position = np.full(2, self.target, dtype=np.float32) + zeros = np.zeros(2, dtype=np.float32) + return ControllerStep( + command=JointServoCommand(position, zeros, zeros, np.ones(2, dtype=np.float32), zeros), + complete=self.complete_after is not None and self.steps > self.complete_after, + ) + + +def _robot() -> RobotSpec: + return RobotSpec( + base_link_name="base", + joint_names=("a", "b"), + default_joint_position=np.zeros(2, dtype=np.float32), + position_lower=np.full(2, -2, dtype=np.float32), + position_upper=np.full(2, 2, dtype=np.float32), + torque_limit=np.ones(2, dtype=np.float32), + ) + + +def _state() -> RobotState: + return RobotState( + sample_time_ns=0, + receive_time_ns=0, + joint_position=np.zeros(2, dtype=np.float32), + joint_velocity=np.zeros(2, dtype=np.float32), + base_orientation_xyzw=np.asarray([0, 0, 0, 1], dtype=np.float32), + base_angular_velocity=np.zeros(3, dtype=np.float32), + base_linear_acceleration=np.zeros(3, dtype=np.float32), + ) + + +def _context(step: int, intent: str | None = None) -> ControlContext[OperatorCommand]: + intents = frozenset() if intent is None else frozenset({intent}) + return ControlContext(step, step * 0.1, OperatorCommand(intents), 0.1) + + +def _machine(initial_state: str = "walk") -> RobotHotSwappingController: + robot = _robot() + passive = _State("passive", robot.joint_names, 0.0) + fixstand = _State("fixstand", robot.joint_names, 0.1) + walk = _State("walk", robot.joint_names, 0.2) + wave = _State("wave", robot.joint_names, 0.3, complete_after=1) + bow = _State("bow", robot.joint_names, 0.4, complete_after=1) + preparation = JointServoPreparationState(robot) + preparation_spec = SkillPreparationSpec( + joint_position=np.zeros(2, dtype=np.float32), + kp=np.ones(2, dtype=np.float32), + kd=np.ones(2, dtype=np.float32), + ramp_duration_s=0.1, + settle_duration_s=0.0, + ) + return RobotHotSwappingController( + joint_names=robot.joint_names, + baseline=BaselineStates(passive, fixstand, walk), + preparation=preparation, + skills=( + SkillModule("wave", wave, preparation_spec, "request_wave"), + SkillModule("bow", bow, preparation_spec, "request_bow"), + ), + initial_state=initial_state, + baseline_transitions=( + TransitionRule("passive", "fixstand", ImmediateTransition), + TransitionRule("fixstand", "walk", ImmediateTransition), + TransitionRule("walk", "fixstand", ImmediateTransition), + ), + passive_transition=ImmediateTransition, + ) + + +def test_skill_request_uses_one_shared_preparation_state_then_returns_to_walk(): + machine = _machine() + state = _state() + machine.reset(state) + + machine.step(state, _context(0)) + machine.step(state, _context(1, "request_wave")) + assert machine.current_state == "skill_preparation" + assert machine.pending_skill == "wave" + + machine.step(state, _context(2)) + assert machine.current_state == "wave" + machine.step(state, _context(3)) + machine.step(state, _context(4)) + assert machine.current_state == "walk" + assert machine.pending_skill is None + + +def test_multiple_skills_share_preparation_and_select_the_pending_destination(): + machine = _machine() + state = _state() + machine.reset(state) + + preparation = machine.states["skill_preparation"] + machine.step(state, _context(0)) + machine.step(state, _context(1, "request_bow")) + assert machine.states["skill_preparation"] is preparation + assert machine.current_state == "skill_preparation" + assert machine.pending_skill == "bow" + + machine.step(state, _context(2)) + assert machine.current_state == "bow" + + +def test_skill_request_is_ignored_until_walk_and_passive_preempts_preparation(): + machine = _machine(initial_state="fixstand") + state = _state() + machine.reset(state) + + machine.step(state, _context(0, "request_wave")) + assert machine.current_state == "fixstand" + assert machine.pending_skill is None + + machine.step(state, _context(1, "request_walk")) + machine.step(state, _context(2, "request_wave")) + assert machine.current_state == "skill_preparation" + machine.step(state, _context(3, "request_passive")) + assert machine.current_state == "passive" + assert machine.pending_skill is None diff --git a/motrix_deploy/tests/test_operator_binding.py b/motrix_deploy/tests/test_operator_binding.py new file mode 100644 index 00000000..541bf337 --- /dev/null +++ b/motrix_deploy/tests/test_operator_binding.py @@ -0,0 +1,91 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +import numpy as np + +from motrix_deploy.operator import ( + ButtonGestureCfg, + OperatorCommandBinding, + OperatorProfileCfg, + PlanarVelocityOperatorCfg, +) +from motrix_env_core.input import GamePadDevice, KeyboardDevice + + +class _Keyboard(KeyboardDevice): + def __init__(self, frames): + self.frames = iter(frames) + self.current = frozenset() + self.previous = frozenset() + + def poll(self): + self.previous = self.current + self.current = frozenset(next(self.frames)) + + def is_key_down(self, key): + return key in self.current and key not in self.previous + + def is_key_up(self, key): + return key not in self.current and key in self.previous + + def is_pressing(self, key): + return key in self.current + + +class _GamePad(GamePadDevice): + def __init__(self, frames): + self.frames = iter(frames) + self.current = ({}, frozenset()) + self.previous = self.current + + def poll(self): + self.previous = self.current + self.current = next(self.frames) + + def axis_value(self, axis): + return self.current[0].get(axis, 0.0) + + def is_button_down(self, button): + return button in self.current[1] and button not in self.previous[1] + + def is_button_up(self, button): + return button not in self.current[1] and button in self.previous[1] + + def is_button_pressing(self, button): + return button in self.current[1] + + +def test_keyboard_binding_produces_intent_and_velocity_from_one_polled_frame(): + device = _Keyboard([set(), {"1", "w"}]) + profile = OperatorProfileCfg(intents={"request_walk": ButtonGestureCfg(pressed="1")}) + binding = OperatorCommandBinding(device, profile, command_lower=(-1, -2, -3), command_upper=(1, 2, 3)) + assert not binding.read_command().intents + command = binding.read_command() + assert command.intents == frozenset({"request_walk"}) + np.testing.assert_array_equal(command.planar_velocity.values, [[1, 0, 0]]) + + +def test_gamepad_binding_applies_deadzone_inversion_range_and_gesture(): + device = _GamePad( + [ + ({}, frozenset()), + ({"ly": 1.0, "lx": 0.5, "rx": -1.0}, frozenset({"RB", "X"})), + ] + ) + profile = OperatorProfileCfg( + intents={"request_walk": ButtonGestureCfg(held=["RB"], pressed="X")}, + planar_velocity=PlanarVelocityOperatorCfg( + linear_x_axis="ly", + linear_y_axis="lx", + yaw_axis="rx", + invert_linear_y=True, + invert_yaw=True, + deadzone=0.0, + range_scale=[0.25, 0.5, 1.0], + ), + ) + binding = OperatorCommandBinding(device, profile, command_lower=(-1, -2, -3), command_upper=(1, 2, 3)) + binding.read_command() + command = binding.read_command() + assert command.intents == frozenset({"request_walk"}) + np.testing.assert_array_equal(command.planar_velocity.values, [[0.25, -0.5, 3.0]]) diff --git a/motrix_deploy/tests/test_package.py b/motrix_deploy/tests/test_package.py index 5005b144..13c0f403 100644 --- a/motrix_deploy/tests/test_package.py +++ b/motrix_deploy/tests/test_package.py @@ -3,11 +3,24 @@ """Package boundary and command-line smoke tests.""" +import re import subprocess import sys +from pathlib import Path import pytest +import motrix_deploy + +_PYPROJECT = Path(__file__).resolve().parents[1] / "pyproject.toml" + + +def test_package_version() -> None: + """The boundary version mirrors the release version declared in pyproject.toml.""" + declared = re.search(r'^version = "(.+)"$', _PYPROJECT.read_text(), re.MULTILINE) + assert declared is not None, f"Missing version in {_PYPROJECT}" + assert motrix_deploy.__version__ == declared.group(1) + @pytest.mark.integration def test_cli_help() -> None: diff --git a/motrix_deploy/tests/test_state_machine.py b/motrix_deploy/tests/test_state_machine.py new file mode 100644 index 00000000..c001c772 --- /dev/null +++ b/motrix_deploy/tests/test_state_machine.py @@ -0,0 +1,157 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +from dataclasses import dataclass + +import numpy as np + +from motrix_deploy.contracts import JointServoCommand, RobotState +from motrix_deploy.operator import OperatorCommand +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy.runtime.lifecycle import ControllerStep +from motrix_deploy.runtime.state_machine import ( + ImmediateTransition, + SmoothServoTransition, + StateMachineController, + TransitionRule, +) + + +@dataclass +class _State: + name: str + joint_names: tuple[str, ...] + target: tuple[float, ...] + kp: float + complete_after: int | None = None + + def reset(self, state: RobotState) -> None: + del state + self.steps = 0 + + def request_stop(self) -> None: + pass + + def step(self, state, context): + del state, context + target = np.asarray(self.target, dtype=np.float32) + zeros = np.zeros_like(target) + self.steps += 1 + return ControllerStep( + command=JointServoCommand(target, zeros, zeros, np.full_like(target, self.kp), np.full_like(target, 1.0)), + complete=self.complete_after is not None and self.steps > self.complete_after, + policy_tick=self.name == "walk", + ) + + +def _robot_state() -> RobotState: + return RobotState( + sample_time_ns=0, + receive_time_ns=0, + joint_position=np.zeros(2, dtype=np.float32), + joint_velocity=np.zeros(2, dtype=np.float32), + base_orientation_xyzw=np.asarray([0, 0, 0, 1], dtype=np.float32), + base_angular_velocity=np.zeros(3, dtype=np.float32), + base_linear_acceleration=np.zeros(3, dtype=np.float32), + ) + + +def _context(step, elapsed, intent=None): + command = OperatorCommand(frozenset() if intent is None else frozenset({intent})) + return ControlContext(step, elapsed, command, 0.1) + + +def test_transition_blends_live_target_and_reorders_by_joint_name(): + passive = _State("passive", ("hip", "knee"), (0.0, 0.0), 0.0) + walk = _State("walk", ("knee", "hip"), (2.0, 1.0), 10.0) + machine = StateMachineController( + joint_names=("hip", "knee"), + states={state.name: state for state in (passive, walk)}, + initial_state="passive", + transitions=(TransitionRule("passive", "walk", lambda: SmoothServoTransition(0.5)),), + intent_targets={"request_walk": "walk"}, + emergency_state="passive", + ) + state = _robot_state() + machine.reset(state) + first = machine.step(state, _context(0, 0.0)) + start = machine.step(state, _context(1, 0.1, "request_walk")) + middle = machine.step(state, _context(2, 0.35)) + end = machine.step(state, _context(3, 0.6)) + np.testing.assert_array_equal(first.command.joint_position, [0, 0]) + np.testing.assert_array_equal(start.command.joint_position, [0, 0]) + np.testing.assert_allclose(middle.command.joint_position, [0.5, 1.0], atol=1e-6) + np.testing.assert_array_equal(end.command.joint_position, [1, 2]) + assert middle.policy_tick and machine.current_state == "walk" + + +def test_emergency_preempts_smooth_transition_immediately(): + states = { + "fixstand": _State("fixstand", ("a", "b"), (1.0, 1.0), 20.0), + "walk": _State("walk", ("a", "b"), (2.0, 2.0), 10.0), + "emergency": _State("emergency", ("a", "b"), (0.0, 0.0), 0.0), + } + machine = StateMachineController( + joint_names=("a", "b"), + states=states, + initial_state="fixstand", + transitions=( + TransitionRule("fixstand", "walk", lambda: SmoothServoTransition(0.5)), + TransitionRule("walk", "emergency", ImmediateTransition), + ), + intent_targets={"request_walk": "walk"}, + emergency_state="emergency", + ) + state = _robot_state() + machine.reset(state) + machine.step(state, _context(0, 0.0)) + machine.step(state, _context(1, 0.1, "request_walk")) + emergency = machine.step(state, _context(2, 0.2, "request_emergency")) + np.testing.assert_array_equal(emergency.command.kp, [0, 0]) + assert machine.current_state == "emergency" and machine.target_state is None + + +def test_completed_policy_returns_to_safe_intermediate_state(): + fixstand = _State("fixstand", ("a", "b"), (1.0, 1.0), 20.0) + motion = _State("motion", ("a", "b"), (2.0, 2.0), 10.0, complete_after=1) + machine = StateMachineController( + joint_names=("a", "b"), + states={state.name: state for state in (fixstand, motion)}, + initial_state="motion", + transitions=(TransitionRule("motion", "fixstand", lambda: SmoothServoTransition(0.5)),), + intent_targets={}, + completion_targets={"motion": "fixstand"}, + emergency_state="fixstand", + ) + state = _robot_state() + machine.reset(state) + assert not machine.step(state, _context(0, 0.0)).complete + assert machine.step(state, _context(1, 0.1)).command is not None + assert machine.target_state == "fixstand" + + +def test_policy_switch_is_routed_through_safe_intermediate_state(): + states = { + name: _State(name, ("a", "b"), target, 10.0) + for name, target in (("walk", (1.0, 1.0)), ("fixstand", (0.0, 0.0)), ("motion", (2.0, 2.0))) + } + machine = StateMachineController( + joint_names=("a", "b"), + states=states, + initial_state="walk", + transitions=( + TransitionRule("walk", "fixstand", lambda: SmoothServoTransition(0.1)), + TransitionRule("fixstand", "motion", lambda: SmoothServoTransition(0.1)), + ), + intent_targets={"request_motion": "motion"}, + emergency_state="fixstand", + ) + state = _robot_state() + machine.reset(state) + machine.step(state, _context(0, 0.0)) + machine.step(state, _context(1, 0.1, "request_motion")) + assert machine.target_state == "fixstand" + machine.step(state, _context(2, 0.2)) + assert machine.current_state == "fixstand" + machine.step(state, _context(3, 0.3)) + assert machine.target_state == "motion" diff --git a/motrix_deploy_tasks/pyproject.toml b/motrix_deploy_tasks/pyproject.toml index bcab4fba..238886a9 100644 --- a/motrix_deploy_tasks/pyproject.toml +++ b/motrix_deploy_tasks/pyproject.toml @@ -21,11 +21,15 @@ dependencies = [ [project.entry-points."motrix_deploy.tasks"] "go2_walk/v1" = "motrix_deploy_tasks.tasks.go2_walk:Go2WalkDeployTask" "g1_wbt/v1" = "motrix_deploy_tasks.tasks.g1_wbt:G1WbtDeployTask" +"g1_walk/v1" = "motrix_deploy_tasks.tasks.g1_walk:G1WalkDeployTask" [project.entry-points."motrix_deploy.envs"] flat = "motrix_deploy_tasks.envs.flat:build_scene" rough = "motrix_deploy_tasks.envs.rough:build_scene" +[project.entry-points."motrix_deploy.programs"] +g1-hot-swapping = "motrix_deploy_tasks.programs.g1_hot_swapping:create_program" + [tool.uv.build-backend] module-name = ["motrix_deploy_tasks", "hydra_plugins"] namespace = true diff --git a/motrix_deploy_tasks/src/hydra_plugins/motrix_deploy_tasks_searchpath.py b/motrix_deploy_tasks/src/hydra_plugins/motrix_deploy_tasks_searchpath.py index 6285285b..18beba10 100644 --- a/motrix_deploy_tasks/src/hydra_plugins/motrix_deploy_tasks_searchpath.py +++ b/motrix_deploy_tasks/src/hydra_plugins/motrix_deploy_tasks_searchpath.py @@ -6,6 +6,8 @@ from hydra.core.config_search_path import ConfigSearchPath from hydra.plugins.search_path_plugin import SearchPathPlugin +from motrix_deploy_tasks.programs.g1_hot_swapping import install_program_schema + class MotrixDeployTasksSearchPathPlugin(SearchPathPlugin): def manipulate_search_path(self, search_path: ConfigSearchPath) -> None: @@ -14,4 +16,5 @@ def manipulate_search_path(self, search_path: ConfigSearchPath) -> None: if any( entry.provider == "main" and entry.path == "pkg://motrix_deploy.config" for entry in search_path.get_path() ): + install_program_schema() search_path.append(provider="motrix-deploy-tasks", path="pkg://motrix_deploy_tasks.config") diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/__init__.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/__init__.py index 50ae5b38..79daea2f 100644 --- a/motrix_deploy_tasks/src/motrix_deploy_tasks/__init__.py +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/__init__.py @@ -3,4 +3,6 @@ """Concrete deployment tasks discovered through installed package entry points.""" -__version__ = "0.3.0" +from importlib import metadata + +__version__ = metadata.version("motrix-deploy-tasks") diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/config/program/g1-hot-swapping.yaml b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/program/g1-hot-swapping.yaml new file mode 100644 index 00000000..aff1424b --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/program/g1-hot-swapping.yaml @@ -0,0 +1,13 @@ +defaults: + - _g1_hot_swapping_schema + - _self_ + +walk_artifact: artifacts/g1-flat.deploy +wbt_artifact: artifacts/g1-wbt-dance.deploy +policy_transition_duration_s: 0.5 +passive_transition_min_s: 0.5 +passive_transition_max_s: 3.0 +passive_max_joint_speed_rad_s: 0.5 +passive_gain_ramp_s: 0.5 +passive_kd: 3.0 +initial_state: fixstand diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/config/runtime/g1-sim-state-machine.yaml b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/runtime/g1-sim-state-machine.yaml new file mode 100644 index 00000000..64c467e1 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/runtime/g1-sim-state-machine.yaml @@ -0,0 +1,15 @@ +defaults: + - sim + - _self_ + +backend: mujoco +viewer: true +deploy_env_id: flat +robot_id: g1-29dof +sensor_bindings: + base_angular_velocity: base_gyro + base_linear_acceleration: base_accel + base_linear_velocity: null +physics: + dt: 0.002 + solver_iterations: 100 diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/config/runtime/g1-unitree-state-machine.yaml b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/runtime/g1-unitree-state-machine.yaml new file mode 100644 index 00000000..b990271c --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/runtime/g1-unitree-state-machine.yaml @@ -0,0 +1,54 @@ +defaults: + - _unitree_schema + - /unitree_robot@robot: g1-29dof + - _self_ + +backend: unitree +transport: native_cpp +network_interface: ??? +domain_id: 0 +lowcmd_topic: rt/lowcmd +lowstate_topic: rt/lowstate +connect_timeout_s: 5.0 +subscriber_queue_depth: 10 +validate_crc: true + +operator: + intents: + request_fixstand: + held: [LT] + pressed: up + request_walk: + held: [RB] + pressed: X + request_wbt: + held: [RB] + pressed: Y + request_passive: + held: [LT] + pressed: B + request_emergency: + pressed: select + planar_velocity: + linear_x_axis: ly + linear_y_axis: lx + yaw_axis: rx + invert_linear_x: false + invert_linear_y: true + invert_yaw: true + deadzone: 0.12 + range_scale: [0.25, 0.25, 0.25] + deadman_button: null + +robot: + startup_mode: state_machine + mode_pr: 0 + expected_mode_machine: 5 + motor_mode: 1 + command_publish_period_s: 0.001 + command_timeout_s: 0.1 + damping_duration_s: 1.0 + damping_kd: 3.0 + wait_for_remote_buttons: false + allow_lie_down: false + override_policy_gains: false diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/g1-walk-flat/hardware.yaml b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/g1-walk-flat/hardware.yaml new file mode 100644 index 00000000..748687e2 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/g1-walk-flat/hardware.yaml @@ -0,0 +1,57 @@ +# @package _global_ +defaults: + - /runtime@runtime: _unitree_schema + - /unitree_robot@runtime.robot: g1-29dof + - _self_ + +artifact: artifacts/g1-flat.deploy +duration_s: 30.0 + +runtime: + backend: unitree + transport: native_cpp + network_interface: ??? + domain_id: 0 + lowcmd_topic: rt/lowcmd + lowstate_topic: rt/lowstate + connect_timeout_s: 5.0 + subscriber_queue_depth: 10 + validate_crc: true + + operator: + intents: + request_stand: + held: [LT] + pressed: up + enable_policy: + held: [RB] + pressed: X + enter_passive: + held: [LT] + pressed: B + emergency_stop: + pressed: select + planar_velocity: + linear_x_axis: ly + linear_y_axis: lx + yaw_axis: rx + invert_linear_x: false + invert_linear_y: true + invert_yaw: true + deadzone: 0.12 + range_scale: [0.25, 0.25, 0.25] + deadman_button: null + + robot: + mode_pr: 0 + expected_mode_machine: 5 + motor_mode: 1 + command_publish_period_s: 0.001 + command_timeout_s: 0.1 + default_transition_duration_s: 3.0 + damping_duration_s: 1.0 + damping_kd: 3.0 + wait_for_remote_buttons: true + allow_lie_down: false + override_policy_gains: false + # FixStand gains are fixed in motrix_robots.g1_standing; artifact gains own the policy phase. diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/g1-walk-flat/sim.yaml b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/g1-walk-flat/sim.yaml new file mode 100644 index 00000000..1fc87e80 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/g1-walk-flat/sim.yaml @@ -0,0 +1,19 @@ +# @package _global_ +# Skeleton recipe for G1 flat-ground locomotion MuJoCo sim2sim. +defaults: + - /runtime: sim + - _self_ + +artifact: artifacts/g1-flat.deploy + +runtime: + backend: mujoco + deploy_env_id: flat + robot_id: g1-29dof + sensor_bindings: + base_angular_velocity: base_gyro + base_linear_acceleration: base_accel + base_linear_velocity: null + physics: + dt: 0.002 + solver_iterations: 100 diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/g1-wbt-dance/hardware.yaml b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/g1-wbt-dance/hardware.yaml new file mode 100644 index 00000000..05647b28 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/config/task/g1-wbt-dance/hardware.yaml @@ -0,0 +1,37 @@ +# @package _global_ +defaults: + - /runtime@runtime: _unitree_schema + - /unitree_robot@runtime.robot: g1-29dof + - _self_ + +artifact: artifacts/g1-wbt-dance.deploy + +runtime: + backend: unitree + transport: native_cpp + network_interface: ??? + domain_id: 0 + lowcmd_topic: rt/lowcmd + lowstate_topic: rt/lowstate + connect_timeout_s: 5.0 + subscriber_queue_depth: 10 + validate_crc: true + robot: + # FixStand gains are fixed in motrix_robots.g1_standing; artifact gains own the policy phase. + override_policy_gains: false + mode_pr: 0 + expected_mode_machine: 5 + motor_mode: 1 + command_publish_period_s: 0.001 + command_timeout_s: 0.1 + default_transition_duration_s: 3.0 + damping_duration_s: 1.0 + damping_kd: 3.0 + wait_for_remote_buttons: true + start_button: start + enable_button: A + emergency_stop_button: select + allow_lie_down: false + +# Exactly the clip playback budget: 999 frames x 0.02 s. +duration_s: 19.98 diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/programs/__init__.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/programs/__init__.py new file mode 100644 index 00000000..3c9b7aed --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/programs/__init__.py @@ -0,0 +1,4 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Persistent multi-policy deployment programs.""" diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/programs/g1_hot_swapping.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/programs/g1_hot_swapping.py new file mode 100644 index 00000000..fd558367 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/programs/g1_hot_swapping.py @@ -0,0 +1,194 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Load and validate the G1 Walk/WBT hot-swapping program.""" + +from dataclasses import dataclass, field +from pathlib import Path + +import numpy as np +from hydra.core.config_store import ConfigStore + +from motrix_deploy.artifact.io import read_artifact +from motrix_deploy.contracts import RobotSpec +from motrix_deploy.errors import ValidationError +from motrix_deploy.operator import ButtonGestureCfg, OperatorProfileCfg, PlanarVelocityOperatorCfg +from motrix_deploy.policy import OnnxPolicy +from motrix_deploy.program import DeploymentProgram +from motrix_deploy.robot.g1_standing import resolve_g1_standing_gains +from motrix_deploy.runtime.preparation import SkillPreparationSpec +from motrix_deploy_tasks.state_machines.g1_states import ( + G1HotSwappingController, + G1TransitionSettings, + G1WalkState, + G1WbtState, +) +from motrix_deploy_tasks.tasks.g1_walk import G1WalkTaskSpec +from motrix_deploy_tasks.tasks.g1_wbt import G1WbtTaskSpec, load_g1_wbt_motion + + +def _default_operator() -> OperatorProfileCfg: + return OperatorProfileCfg( + intents={ + "request_fixstand": ButtonGestureCfg(pressed="1"), + "request_walk": ButtonGestureCfg(pressed="2"), + "request_wbt": ButtonGestureCfg(pressed="3"), + "request_passive": ButtonGestureCfg(pressed="0"), + "request_emergency": ButtonGestureCfg(pressed="x"), + }, + planar_velocity=PlanarVelocityOperatorCfg(), + ) + + +@dataclass +class G1HotSwappingProgramCfg: + """Artifact paths, transition timing and backend-neutral operator defaults.""" + + name: str = "g1-hot-swapping" + walk_artifact: str = "artifacts/g1-flat.deploy" + wbt_artifact: str = "artifacts/g1-wbt-dance.deploy" + policy_transition_duration_s: float = 0.5 + passive_transition_min_s: float = 0.5 + passive_transition_max_s: float = 3.0 + passive_max_joint_speed_rad_s: float = 0.5 + passive_gain_ramp_s: float = 0.5 + passive_kd: float = 3.0 + initial_state: str = "fixstand" + operator: OperatorProfileCfg = field(default_factory=_default_operator) + + def __post_init__(self) -> None: + if self.name != "g1-hot-swapping": + raise ValueError(f"unsupported program name: {self.name!r}") + if self.initial_state not in {"passive", "fixstand"}: + raise ValueError("initial_state must be passive or fixstand") + if not self.walk_artifact or not self.wbt_artifact: + raise ValueError("walk_artifact and wbt_artifact must be non-empty paths") + for name in ( + "policy_transition_duration_s", + "passive_transition_min_s", + "passive_transition_max_s", + "passive_max_joint_speed_rad_s", + "passive_gain_ramp_s", + ): + value = getattr(self, name) + if not np.isfinite(value) or value <= 0: + raise ValueError(f"{name} must be positive and finite") + if self.passive_transition_min_s > self.passive_transition_max_s: + raise ValueError("passive transition minimum must not exceed maximum") + if self.passive_kd < 0: + raise ValueError("passive damping gain must be non-negative") + + +def install_program_schema() -> None: + ConfigStore.instance().store(group="program", name="_g1_hot_swapping_schema", node=G1HotSwappingProgramCfg) + + +def _indices(source: tuple[str, ...], target: tuple[str, ...]) -> np.ndarray: + if set(source) != set(target): + raise ValidationError("program.robot.joint_names", f"joint set {sorted(target)}", sorted(source)) + return np.asarray([source.index(name) for name in target], dtype=np.int64) + + +def _validate_robot_compatibility(canonical: RobotSpec, candidate: RobotSpec) -> None: + if canonical.base_link_name != candidate.base_link_name: + raise ValidationError("program.robot.base_link_name", canonical.base_link_name, candidate.base_link_name) + order = _indices(candidate.joint_names, canonical.joint_names) + for name in ("default_joint_position", "position_lower", "position_upper", "torque_limit"): + expected = getattr(canonical, name) + actual = getattr(candidate, name)[order] + if not np.allclose(expected, actual, atol=1e-6, rtol=0): + raise ValidationError(f"program.robot.{name}", expected.tolist(), actual.tolist()) + + +def _reorder(values: object, source: tuple[str, ...], target: tuple[str, ...]) -> np.ndarray: + return np.asarray(values, dtype=np.float32)[_indices(source, target)].astype(np.float32) + + +def create_program(config: G1HotSwappingProgramCfg) -> DeploymentProgram: + """Preload both artifacts and construct one backend-neutral persistent controller.""" + if not isinstance(config, G1HotSwappingProgramCfg): + raise TypeError(f"expected G1HotSwappingProgramCfg, got {type(config).__name__}") + walk_artifact = read_artifact(Path(config.walk_artifact)) + wbt_artifact = read_artifact(Path(config.wbt_artifact)) + walk_manifest = walk_artifact.manifest + wbt_manifest = wbt_artifact.manifest + if not isinstance(walk_manifest.task, G1WalkTaskSpec): + raise ValidationError("program.walk_artifact.task", G1WalkTaskSpec.task_name, walk_manifest.task.name) + if not isinstance(wbt_manifest.task, G1WbtTaskSpec): + raise ValidationError("program.wbt_artifact.task", G1WbtTaskSpec.task_name, wbt_manifest.task.name) + if walk_manifest.control != wbt_manifest.control: + raise ValidationError("program.control", walk_manifest.control, wbt_manifest.control) + robot = walk_manifest.robot + _validate_robot_compatibility(robot, wbt_manifest.robot) + + walk_policy = OnnxPolicy(walk_artifact.policy_path, walk_manifest.policy.input, walk_manifest.policy.output) + wbt_policy = OnnxPolicy(wbt_artifact.policy_path, wbt_manifest.policy.input, wbt_manifest.policy.output) + motion = load_g1_wbt_motion(wbt_manifest.task, wbt_artifact) + walk = G1WalkState(walk_manifest.task, robot, walk_policy, robot.joint_names) + wbt = G1WbtState( + wbt_manifest.task, + wbt_manifest.robot, + motion, + wbt_policy, + robot.joint_names, + takeover_hold_steps=max(1, round(config.policy_transition_duration_s / walk_manifest.control.period_s)), + ) + wbt_spec = wbt_manifest.task + wbt_robot = wbt_manifest.robot + fixstand_position = _reorder( + wbt_robot.default_joint_position + if wbt_spec.preparation_joint_position is None + else wbt_spec.preparation_joint_position, + wbt_robot.joint_names, + robot.joint_names, + ) + # FixStand is a fixed posture owned by the deploy, not by any policy. + fixstand_kp, fixstand_kd = resolve_g1_standing_gains(robot.joint_names) + # Skill preparation stays per-artifact: every skill ships its own stand-up recipe. + wbt_preparation_kp = _reorder( + wbt_spec.kp if wbt_spec.preparation_kp is None else wbt_spec.preparation_kp, + wbt_robot.joint_names, + robot.joint_names, + ) + wbt_preparation_kd = _reorder( + wbt_spec.kd if wbt_spec.preparation_kd is None else wbt_spec.preparation_kd, + wbt_robot.joint_names, + robot.joint_names, + ) + settings = G1TransitionSettings( + policy_duration_s=config.policy_transition_duration_s, + passive_duration_min_s=config.passive_transition_min_s, + passive_duration_max_s=config.passive_transition_max_s, + passive_max_joint_speed_rad_s=config.passive_max_joint_speed_rad_s, + passive_gain_ramp_s=config.passive_gain_ramp_s, + ) + controller = G1HotSwappingController( + robot=robot, + walk=walk, + wbt=wbt, + fixstand_joint_position=fixstand_position, + fixstand_kp=fixstand_kp, + fixstand_kd=fixstand_kd, + passive_kd=np.full(robot.joint_count, config.passive_kd, dtype=np.float32), + wbt_preparation=SkillPreparationSpec( + joint_position=fixstand_position, + kp=wbt_preparation_kp, + kd=wbt_preparation_kd, + ramp_duration_s=wbt_spec.preparation_ramp_duration_s, + settle_duration_s=wbt_spec.preparation_settle_duration_s, + timeout_s=wbt_spec.preparation_timeout_s, + ), + initial_state=config.initial_state, + settings=settings, + ) + return DeploymentProgram( + robot=robot, + control=walk_manifest.control, + controller=controller, + command_lower=walk.processor.command_lower, + command_upper=walk.processor.command_upper, + operator=config.operator, + ) + + +__all__ = ["G1HotSwappingProgramCfg", "create_program", "install_program_schema"] diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/state_machines/__init__.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/state_machines/__init__.py new file mode 100644 index 00000000..3494df42 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/state_machines/__init__.py @@ -0,0 +1,4 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Task-owned persistent deployment state machines.""" diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/state_machines/g1_states.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/state_machines/g1_states.py new file mode 100644 index 00000000..a15bc915 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/state_machines/g1_states.py @@ -0,0 +1,277 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Persistent G1 deployment states (passive/fixstand/walk/wbt) and their controller.""" + +from __future__ import annotations + +from dataclasses import dataclass + +import numpy as np + +from motrix_deploy.contracts import FloatArray, JointServoCommand, RobotSpec, RobotState, float32_array +from motrix_deploy.operator import OperatorCommand +from motrix_deploy.policy import Policy +from motrix_deploy.runtime.context import ControlContext +from motrix_deploy.runtime.control import PolicyController +from motrix_deploy.runtime.hot_swapping import BaselineStates, RobotHotSwappingController, SkillModule +from motrix_deploy.runtime.lifecycle import ControllerStep +from motrix_deploy.runtime.preparation import JointServoPreparationState, SkillPreparationSpec +from motrix_deploy.runtime.state_machine import ( + AdaptiveServoTransition, + SmoothServoTransition, + TransitionRule, +) +from motrix_deploy_tasks.tasks.g1_walk import G1WalkPolicyProcessor, G1WalkTaskSpec +from motrix_deploy_tasks.tasks.g1_wbt import G1WbtMotion, G1WbtPolicyProcessor, G1WbtTaskSpec +from motrix_env_core.input import PlanarVelocityCommand + + +def _vector(value: object, *, path: str, joint_count: int) -> FloatArray: + return float32_array(np.asarray(value, dtype=np.float32), path=path, shape=(joint_count,)) + + +def _state_in_joint_order( + state: RobotState, source_names: tuple[str, ...], target_names: tuple[str, ...] +) -> RobotState: + if source_names == target_names: + return state + indices = np.asarray([source_names.index(name) for name in target_names], dtype=np.int64) + return RobotState( + sample_time_ns=state.sample_time_ns, + receive_time_ns=state.receive_time_ns, + joint_position=state.joint_position[indices].astype(np.float32), + joint_velocity=state.joint_velocity[indices].astype(np.float32), + base_orientation_xyzw=state.base_orientation_xyzw, + base_angular_velocity=state.base_angular_velocity, + base_linear_acceleration=state.base_linear_acceleration, + base_linear_velocity=state.base_linear_velocity, + ) + + +class G1PassiveState: + """Persistent zero-stiffness damping without ending the control session.""" + + name = "passive" + + def __init__( + self, + joint_names: tuple[str, ...], + kd: FloatArray, + position_lower: FloatArray, + position_upper: FloatArray, + ) -> None: + self.joint_names = joint_names + self._kd = _vector(kd, path="g1.passive.kd", joint_count=len(joint_names)) + self._position_lower = _vector(position_lower, path="g1.passive.position_lower", joint_count=len(joint_names)) + self._position_upper = _vector(position_upper, path="g1.passive.position_upper", joint_count=len(joint_names)) + self._zeros = np.zeros(len(joint_names), dtype=np.float32) + + def reset(self, state: RobotState) -> None: + del state + + def request_stop(self) -> None: + pass + + def step(self, state: RobotState, context: ControlContext[OperatorCommand]) -> ControllerStep: + del context + target = np.clip(state.joint_position, self._position_lower, self._position_upper).astype(np.float32) + return ControllerStep(command=JointServoCommand(target, self._zeros, self._zeros, self._zeros, self._kd)) + + +class G1FixStandState: + """Persistent joint-name-defined standing posture and support gains.""" + + name = "fixstand" + + def __init__( + self, + joint_names: tuple[str, ...], + joint_position: FloatArray, + kp: FloatArray, + kd: FloatArray, + ) -> None: + joint_count = len(joint_names) + self.joint_names = joint_names + self._target = _vector(joint_position, path="g1.fixstand.joint_position", joint_count=joint_count) + self._kp = _vector(kp, path="g1.fixstand.kp", joint_count=joint_count) + self._kd = _vector(kd, path="g1.fixstand.kd", joint_count=joint_count) + self._zeros = np.zeros(joint_count, dtype=np.float32) + + def reset(self, state: RobotState) -> None: + del state + + def request_stop(self) -> None: + pass + + def step(self, state: RobotState, context: ControlContext[OperatorCommand]) -> ControllerStep: + del state, context + return ControllerStep(command=JointServoCommand(self._target, self._zeros, self._zeros, self._kp, self._kd)) + + +class G1WalkState: + """Persistent locomotion backed by the exported Walk processor and ONNX policy.""" + + name = "walk" + + def __init__( + self, + spec: G1WalkTaskSpec, + robot: RobotSpec, + policy: Policy, + canonical_joint_names: tuple[str, ...], + ) -> None: + self.joint_names = robot.joint_names + self._canonical_joint_names = canonical_joint_names + self.processor = G1WalkPolicyProcessor(spec, robot) + self.controller = PolicyController(self.processor, policy) + self._zero_velocity = PlanarVelocityCommand(np.zeros((1, 3), dtype=np.float32)) + + def _state(self, state: RobotState) -> RobotState: + return _state_in_joint_order(state, self._canonical_joint_names, self.joint_names) + + def reset(self, state: RobotState) -> None: + self.controller.steps = None + self.controller.reset(self._state(state)) + + def request_stop(self) -> None: + self.controller.request_stop() + + def step(self, state: RobotState, context: ControlContext[OperatorCommand]) -> ControllerStep: + operator = context.command + velocity = ( + self._zero_velocity if operator is None or operator.planar_velocity is None else operator.planar_velocity + ) + local = ControlContext(context.step, context.elapsed_time_s, velocity, context.dt_s) + return self.controller.step(self._state(state), local) + + +class G1WbtState: + """Finite WBT playback with a frame-zero policy warm-up during takeover.""" + + name = "wbt" + + def __init__( + self, + spec: G1WbtTaskSpec, + robot: RobotSpec, + motion: G1WbtMotion, + policy: Policy, + canonical_joint_names: tuple[str, ...], + *, + takeover_hold_steps: int, + ) -> None: + if takeover_hold_steps < 1: + raise ValueError("takeover_hold_steps must be at least one policy interval") + self.joint_names = robot.joint_names + self._canonical_joint_names = canonical_joint_names + self.motion = motion + self.processor = G1WbtPolicyProcessor(spec, robot, motion) + self.controller = PolicyController(self.processor, policy) + self._takeover_hold_steps = takeover_hold_steps + self._ticks = 0 + self._playback_frame = 0 + self._playback_started = False + + def _state(self, state: RobotState) -> RobotState: + return _state_in_joint_order(state, self._canonical_joint_names, self.joint_names) + + def reset(self, state: RobotState) -> None: + self._ticks = 0 + self._playback_frame = 0 + self._playback_started = False + local = self._state(state) + self.processor.reset_reference() + # Align the clip's heading with the robot's heading at takeover. + self.processor.align_reference_to(local) + self.controller.steps = None + self.controller.reset(local) + + def request_stop(self) -> None: + self.controller.request_stop() + + def step(self, state: RobotState, context: ControlContext[OperatorCommand]) -> ControllerStep: + if not self._playback_started and self._ticks >= self._takeover_hold_steps: + self._playback_started = True + self.controller.steps = self.controller.completed_steps + len(self.motion) + frame = self._playback_frame if self._playback_started else 0 + self.processor.set_reference_frame(min(frame, len(self.motion) - 1)) + local = ControlContext(context.step, context.elapsed_time_s, None, context.dt_s) + decision = self.controller.step(self._state(state), local) + self._ticks += 1 + if self._playback_started and decision.policy_tick: + self._playback_frame += 1 + return decision + + +@dataclass(frozen=True) +class G1TransitionSettings: + """Durations for normal policy takeover and safe posture recovery.""" + + policy_duration_s: float = 0.5 + passive_duration_min_s: float = 0.5 + passive_duration_max_s: float = 3.0 + passive_max_joint_speed_rad_s: float = 0.5 + passive_gain_ramp_s: float = 0.5 + + +class G1HotSwappingController(RobotHotSwappingController): + """Bind G1 baseline states and WBT as one standard hot-swappable skill.""" + + def __init__( + self, + *, + robot: RobotSpec, + walk: G1WalkState, + wbt: G1WbtState, + fixstand_joint_position: FloatArray, + fixstand_kp: FloatArray, + fixstand_kd: FloatArray, + passive_kd: FloatArray, + wbt_preparation: SkillPreparationSpec, + initial_state: str = "fixstand", + settings: G1TransitionSettings = G1TransitionSettings(), + ) -> None: + joint_names = robot.joint_names + passive = G1PassiveState(joint_names, passive_kd, robot.position_lower, robot.position_upper) + fixstand = G1FixStandState(joint_names, fixstand_joint_position, fixstand_kp, fixstand_kd) + baseline_transitions = ( + TransitionRule( + "passive", + "fixstand", + lambda: AdaptiveServoTransition( + settings.passive_duration_min_s, + settings.passive_duration_max_s, + settings.passive_max_joint_speed_rad_s, + ), + ), + TransitionRule("fixstand", "passive", lambda: SmoothServoTransition(settings.passive_gain_ramp_s)), + TransitionRule("fixstand", "walk", lambda: SmoothServoTransition(settings.policy_duration_s)), + TransitionRule("walk", "fixstand", lambda: SmoothServoTransition(settings.policy_duration_s)), + ) + super().__init__( + joint_names=joint_names, + baseline=BaselineStates(passive=passive, fixstand=fixstand, walk=walk), + preparation=JointServoPreparationState(robot), + skills=( + SkillModule( + name="wbt", + state=wbt, + preparation=wbt_preparation, + request_intent="request_wbt", + ), + ), + initial_state=initial_state, + baseline_transitions=baseline_transitions, + passive_transition=lambda: SmoothServoTransition(settings.passive_gain_ramp_s), + ) + + +__all__ = [ + "G1FixStandState", + "G1HotSwappingController", + "G1PassiveState", + "G1TransitionSettings", + "G1WalkState", + "G1WbtState", +] diff --git a/motrix_deploy_tasks/src/motrix_deploy_tasks/tasks/g1_walk.py b/motrix_deploy_tasks/src/motrix_deploy_tasks/tasks/g1_walk.py new file mode 100644 index 00000000..495035c0 --- /dev/null +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/tasks/g1_walk.py @@ -0,0 +1,357 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Typed artifact specification and runtime skeleton for ``g1_walk/v1``.""" + +import math +from typing import Annotated, ClassVar + +import numpy as np +from pydantic import Field, model_validator + +from motrix_deploy.artifact.schema import TaskSpec +from motrix_deploy.contracts import FloatArray, JointServoCommand, RobotSpec, RobotState, float32_array +from motrix_deploy.errors import ValidationError +from motrix_deploy.operator import OperatorProfileCfg +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 ( + BoundedGamePadPlanarVelocityBinding, + CommandBinding, + ConstantPlanarVelocityBinding, + GamePadDevice, + InputDevice, + KeyboardDevice, + KeyboardPlanarVelocityBinding, + PlanarVelocityCommand, +) + +FLOAT32_MAX = float(np.finfo(np.float32).max) +FiniteFloat = Annotated[float, Field(allow_inf_nan=False)] +Float32Float = Annotated[FiniteFloat, Field(ge=-FLOAT32_MAX, le=FLOAT32_MAX)] +NonNegativeFloat = Annotated[FiniteFloat, Field(ge=0)] +PositiveFloat = Annotated[FiniteFloat, Field(gt=0)] +JointVector = Annotated[list[Float32Float], Field(min_length=1)] +NonNegativeJointVector = Annotated[list[Annotated[Float32Float, Field(ge=0)]], Field(min_length=1)] +CommandVector = Annotated[list[Float32Float], Field(min_length=3, max_length=3)] +GaitPhaseOffsets = Annotated[list[Float32Float], Field(min_length=2, max_length=2)] + + +class G1WalkTaskSpec(TaskSpec): + """Self-contained G1 locomotion policy I/O and gait parameters. + + Joint vectors use the canonical order stored in the artifact's + ``RobotSpec.joint_names``. Action bounds apply to raw policy output before + multiplication by ``action_scale`` and addition of the robot default pose. + Command vectors use ``(vx, vy, wz)`` order in SI units. + """ + + task_name: ClassVar[str] = "g1_walk/v1" + + action_scale: NonNegativeJointVector + kp: NonNegativeJointVector + kd: NonNegativeJointVector + action_lower: JointVector + action_upper: JointVector + + command_lower: CommandVector + command_upper: CommandVector + + base_angular_velocity_scale: PositiveFloat + joint_position_scale: PositiveFloat + joint_velocity_scale: PositiveFloat + + gait_period_s: PositiveFloat + gait_phase_offsets_rad: GaitPhaseOffsets + standing_command_threshold: NonNegativeFloat + + @model_validator(mode="after") + def validate_vector_lengths_and_ranges(self) -> "G1WalkTaskSpec": + joint_count = len(self.action_scale) + for key in ("kp", "kd", "action_lower", "action_upper"): + if len(getattr(self, key)) != joint_count: + raise ValueError(f"{key} must have the same length as action_scale ({joint_count})") + + if any(lower > upper for lower, upper in zip(self.action_lower, self.action_upper, strict=True)): + raise ValueError("action_lower must be less than or equal to action_upper") + if any(lower > 0.0 or upper < 0.0 for lower, upper in zip(self.action_lower, self.action_upper, strict=True)): + raise ValueError("raw action range must contain zero for every joint") + + if any(lower > upper for lower, upper in zip(self.command_lower, self.command_upper, strict=True)): + raise ValueError("command_lower must be less than or equal to command_upper") + if any(lower > 0.0 or upper < 0.0 for lower, upper in zip(self.command_lower, self.command_upper, strict=True)): + raise ValueError("velocity command range must contain zero on every axis") + + left, right = self.gait_phase_offsets_rad + phase_difference = (right - left) % (2.0 * math.pi) + if not math.isclose(phase_difference, math.pi, rel_tol=0.0, abs_tol=1e-6): + raise ValueError("gait phase lanes must be separated by pi radians") + return self + + +class G1WalkPolicyProcessor(PolicyProcessor[PlanarVelocityCommand]): + """Reproduce the G1 walk policy observation and joint-servo contracts.""" + + def __init__(self, spec: G1WalkTaskSpec, robot: RobotSpec) -> None: + joint_count = robot.joint_count + if len(spec.action_scale) != joint_count: + raise ValidationError("task.config.action_scale", f"{joint_count} joint values", len(spec.action_scale)) + self.spec = spec + self.robot = robot + shape = (joint_count,) + self._action_scale = _spec_array(spec.action_scale, path="task.config.action_scale", shape=shape) + self._kp = _spec_array(spec.kp, path="task.config.kp", shape=shape) + self._kd = _spec_array(spec.kd, path="task.config.kd", shape=shape) + self._action_lower = _spec_array(spec.action_lower, path="task.config.action_lower", shape=shape) + self._action_upper = _spec_array(spec.action_upper, path="task.config.action_upper", shape=shape) + self._command_lower = _spec_array(spec.command_lower, path="task.config.command_lower", shape=(3,)) + self._command_upper = _spec_array(spec.command_upper, path="task.config.command_upper", shape=(3,)) + self._gait_phase_offsets = _spec_array( + spec.gait_phase_offsets_rad, + path="task.config.gait_phase_offsets_rad", + shape=(2,), + ) + self._base_angular_velocity_scale = np.float32(spec.base_angular_velocity_scale) + self._joint_position_scale = np.float32(spec.joint_position_scale) + self._joint_velocity_scale = np.float32(spec.joint_velocity_scale) + self._gait_period_s = np.float32(spec.gait_period_s) + self._standing_command_threshold = np.float32(spec.standing_command_threshold) + self._previous_action = np.zeros(joint_count, dtype=np.float32) + self._phase = np.float32(0.0) + self._last_step = 0 + + @property + def command_lower(self) -> FloatArray: + return self._command_lower + + @property + def command_upper(self) -> FloatArray: + return self._command_upper + + def reset(self, state: RobotState, context: ControlContext[PlanarVelocityCommand]) -> None: + """Clear raw-action history and restart the deterministic gait clock.""" + + self._validate_state_joint_count(state) + self._previous_action.fill(0.0) + self._phase = np.float32(0.0) + self._last_step = 0 + + def build_observation( + self, + state: RobotState, + context: ControlContext[PlanarVelocityCommand], + ) -> FloatArray: + """Build the exact 100-element actor input used by ``g1-walk-flat``.""" + + self._validate_state_joint_count(state) + velocity = self._validated_velocity(context.command) + self._advance_phase(context) + + linear_speed = float(np.linalg.norm(velocity[:2])) + standing = linear_speed < float(self._standing_command_threshold) and abs(float(velocity[2])) < float( + self._standing_command_threshold + ) + if standing: + phase = np.full(2, np.float32(math.pi), dtype=np.float32) + else: + phase = self._phase + self._gait_phase_offsets + sin_phase = np.sin(phase).astype(np.float32) + cos_phase = np.cos(phase).astype(np.float32) + + observation = np.concatenate( + ( + state.base_angular_velocity * self._base_angular_velocity_scale, + _projected_gravity_body(state.base_orientation_xyzw), + velocity, + (state.joint_position - self.robot.default_joint_position) * self._joint_position_scale, + state.joint_velocity * self._joint_velocity_scale, + self._previous_action, + sin_phase, + cos_phase, + ) + ).astype(np.float32) + expected_shape = (9 + 3 * self.robot.joint_count + 4,) + if observation.shape != expected_shape or not np.all(np.isfinite(observation)): + raise ValidationError("observation", f"finite float32 values with shape {expected_shape}", observation) + return observation + + def process_action(self, action: FloatArray) -> JointServoCommand: + """Clip raw policy output and convert it to canonical joint servos.""" + + shape = (self.robot.joint_count,) + raw = float32_array(action, path="action.raw", shape=shape) + executed = np.clip(raw, self._action_lower, self._action_upper).astype(np.float32) + target = self.robot.default_joint_position + self._action_scale * executed + target = np.clip(target, self.robot.position_lower, self.robot.position_upper).astype(np.float32) + self._previous_action = executed.copy() + zeros = np.zeros(self.robot.joint_count, dtype=np.float32) + return JointServoCommand( + joint_position=target, + joint_velocity=zeros, + feedforward_torque=zeros.copy(), + kp=np.array(self._kp, dtype=np.float32, copy=True), + kd=np.array(self._kd, dtype=np.float32, copy=True), + ) + + def validate_command(self, command: PlanarVelocityCommand | None) -> None: + self._validated_velocity(command) + + def _validated_velocity(self, command: PlanarVelocityCommand | None) -> FloatArray: + if not isinstance(command, PlanarVelocityCommand): + raise ValidationError("command.type", "PlanarVelocityCommand", type(command).__name__) + if command.batch_size != 1: + raise ValidationError("command.batch_size", "1", command.batch_size) + velocity = command.values[0] + if np.any(velocity < self._command_lower) or np.any(velocity > self._command_upper): + raise ValidationError( + "command.velocity", + f"inside [{self._command_lower.tolist()}, {self._command_upper.tolist()}]", + velocity.tolist(), + ) + return velocity + + def _validate_state_joint_count(self, state: RobotState) -> None: + expected = (self.robot.joint_count,) + if state.joint_position.shape != expected or state.joint_velocity.shape != expected: + raise ValidationError( + "state.joints.shape", + str(expected), + (state.joint_position.shape, state.joint_velocity.shape), + ) + + def _advance_phase(self, context: ControlContext[PlanarVelocityCommand]) -> None: + if context.step < self._last_step: + raise ValidationError("context.step", f">= {self._last_step}", context.step) + if not np.isfinite(context.dt_s) or context.dt_s <= 0.0: + raise ValidationError("context.dt_s", "a positive finite value", context.dt_s) + elapsed_steps = context.step - self._last_step + if elapsed_steps: + increment = 2.0 * math.pi * elapsed_steps * context.dt_s / float(self._gait_period_s) + self._phase = np.float32((float(self._phase) + increment) % (2.0 * math.pi)) + self._last_step = context.step + + +def _projected_gravity_body(orientation_xyzw: FloatArray) -> FloatArray: + """Rotate world ``-Z`` into body coordinates for a body-to-world quaternion.""" + + x, y, z, w = orientation_xyzw + return np.asarray( + ( + 2.0 * (y * w - x * z), + -2.0 * (x * w + y * z), + 2.0 * (x * x + y * y) - 1.0, + ), + dtype=np.float32, + ) + + +def _spec_array(value: object, *, path: str, shape: tuple[int, ...]) -> FloatArray: + """Materialize one validated Pydantic numeric vector as immutable float32.""" + + return float32_array(np.asarray(value, dtype=np.float32), path=path, shape=shape) + + +class G1WalkDeployTask(DeployTask[PlanarVelocityCommand]): + """Assemble G1 locomotion policy I/O with an operator command source.""" + + spec_type = G1WalkTaskSpec + + def __init__( + self, + spec: G1WalkTaskSpec, + robot: RobotSpec, + policy: Policy, + steps: int | None = None, + artifact: object | None = None, + ) -> None: + del artifact + self.spec = spec + self.robot = robot + self.policy_io = G1WalkPolicyProcessor(spec, robot) + self.policy_controller = PolicyController(self.policy_io, policy, steps=steps) + self._keyboard_attached = False + self._gamepad_attached = False + self._operator_profile: OperatorProfileCfg | None = None + + def configure_operator(self, profile: OperatorProfileCfg | None) -> None: + """Store task-owned gamepad command wiring from the selected recipe.""" + self._operator_profile = profile + + def prepare( + self, + *, + simulation: bool, + device: InputDevice | None = None, + spawn_pose: str | None = None, + ) -> CommandBinding[PlanarVelocityCommand]: + """Bind viewer keys to velocity commands, or stand still without a keyboard.""" + + del simulation, spawn_pose + command_lower, command_upper = self.command_bounds + if isinstance(device, KeyboardDevice): + self._keyboard_attached = True + self._gamepad_attached = False + return KeyboardPlanarVelocityBinding( + device, + command_lower=command_lower, + command_upper=command_upper, + ) + if isinstance(device, GamePadDevice) and self._operator_profile is not None: + velocity = self._operator_profile.planar_velocity + if velocity is not None: + self._keyboard_attached = False + self._gamepad_attached = True + return BoundedGamePadPlanarVelocityBinding( + device, + linear_x_axis=velocity.linear_x_axis, + linear_y_axis=velocity.linear_y_axis, + yaw_axis=velocity.yaw_axis, + command_lower=command_lower, + command_upper=command_upper, + deadzone=velocity.deadzone, + range_scale=velocity.range_scale, + invert_linear_x=velocity.invert_linear_x, + invert_linear_y=velocity.invert_linear_y, + invert_yaw=velocity.invert_yaw, + deadman_button=velocity.deadman_button, + ) + self._keyboard_attached = False + self._gamepad_attached = False + binding = ConstantPlanarVelocityBinding((0.0, 0.0, 0.0)) + self.validate_command(binding.read_command()) + return binding + + @property + def command_bounds(self) -> tuple[FloatArray, FloatArray]: + return self.policy_io.command_lower, self.policy_io.command_upper + + @property + def operator_hint(self) -> str | None: + if self._keyboard_attached: + return "w/s forward/back a/d left/right q/e turn left/right" + if self._gamepad_attached: + return "G1 remote: LT+Up stand RB+X enable policy LT+B passive select emergency stop" + return None + + 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() + + def validate_command(self, command: PlanarVelocityCommand | None) -> None: + self.policy_io.validate_command(command) + + +__all__ = ["G1WalkDeployTask", "G1WalkPolicyProcessor", "G1WalkTaskSpec"] 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 index af0b5092..786bb528 100644 --- a/motrix_deploy_tasks/src/motrix_deploy_tasks/tasks/g1_wbt.py +++ b/motrix_deploy_tasks/src/motrix_deploy_tasks/tasks/g1_wbt.py @@ -28,6 +28,7 @@ from motrix_deploy.runtime.context import ControlContext from motrix_deploy.runtime.control import PolicyController from motrix_deploy.runtime.lifecycle import ControllerStep +from motrix_deploy.runtime.preparation import JointServoPreparationState, SkillPreparationSpec from motrix_deploy.task import DeployTask FiniteFloat = Annotated[float, Field(allow_inf_nan=False)] @@ -84,7 +85,7 @@ def __len__(self) -> int: return int(np.asarray(self.joint_pos).shape[0]) -def _load_motion(spec: G1WbtTaskSpec, artifact: Artifact | None) -> G1WbtMotion: +def load_g1_wbt_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) @@ -140,6 +141,9 @@ def __init__(self, spec: G1WbtTaskSpec, robot: RobotSpec, motion: G1WbtMotion) - 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) + # Yaw offset between the clip's world frame and the robot's IMU world frame. + # Identity keeps the raw reference (simulation and parity harnesses). + self._reference_alignment = np.asarray((0.0, 0.0, 0.0, 1.0), 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) @@ -153,6 +157,21 @@ def reset_reference(self) -> None: self._reference_frame = 0 self._latest_frame = 0 + def align_reference_to(self, state: RobotState) -> None: + """Rotate the clip's world frame onto the robot's measured heading. + + The IMU has no absolute heading, so feeding ``R(torso)^T @ R(reference)`` + without this leaves a constant yaw offset in the 6D block equal to the + difference between the robot's heading and the clip's frame 0. The policy + then twists to "correct" it (works at one heading, diverges at another). + Mirrors unitree_rl_lab ``State_WBT.cpp``: ``init_quat = yaw(robot) * yaw(ref0)^T``. + """ + torso = self.torso_orientation(state) + self._reference_alignment = _quat_from_yaw(_yaw_of(torso) - _yaw_of(self._reference_quaternion[0])) + + def _aligned_reference(self, frame: int) -> np.ndarray: + return _quat_multiply(self._reference_alignment, self._reference_quaternion[frame]) + def set_reference_frame(self, frame: int) -> None: """Select a reference independently of the continuously running policy.""" if not 0 <= frame < self._frames: @@ -192,9 +211,7 @@ def actor_observation( ``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] - ) + relative_rotation = _rotation_matrix(torso_quaternion_xyzw).T @ _rotation_matrix(self._aligned_reference(frame)) raw_history = self._previous_action if previous_action is None else previous_action return np.concatenate( ( @@ -253,7 +270,7 @@ def process_action(self, action: FloatArray) -> JointServoCommand: def check_termination(self, state: RobotState) -> str | None: frame = self._latest_frame torso = self.torso_orientation(state) - reference = self._reference_quaternion[frame] + reference = self._aligned_reference(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" @@ -267,6 +284,31 @@ def check_termination(self, state: RobotState) -> str | None: return None +def _quat_multiply(a: np.ndarray, b: np.ndarray) -> np.ndarray: + """Hamilton product of two xyzw quaternions.""" + ax, ay, az, aw = (float(value) for value in a) + bx, by, bz, bw = (float(value) for value in b) + return np.asarray( + ( + aw * bx + bw * ax + ay * bz - az * by, + aw * by + bw * ay + az * bx - ax * bz, + aw * bz + bw * az + ax * by - ay * bx, + aw * bw - ax * bx - ay * by - az * bz, + ), + dtype=np.float32, + ) + + +def _yaw_of(quaternion_xyzw: np.ndarray) -> float: + """ZYX yaw of a body-to-world quaternion.""" + x, y, z, w = (float(value) for value in quaternion_xyzw) + return float(np.arctan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z))) + + +def _quat_from_yaw(yaw: float) -> np.ndarray: + return np.asarray((0.0, 0.0, np.sin(0.5 * yaw), np.cos(0.5 * yaw)), dtype=np.float32) + + 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) @@ -292,101 +334,6 @@ class StageResult: 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.""" @@ -433,7 +380,7 @@ def __init__( artifact: Artifact | None = None, ): if motion is None: - motion = _load_motion(spec, artifact) + motion = load_g1_wbt_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) @@ -449,20 +396,24 @@ def __init__( else spec.preparation_joint_position, dtype=np.float32, ).copy() + # Per-artifact stand-up recipe: the policy gains are tracking gains and + # cannot hold a static stance. 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( + self._preparation_spec = SkillPreparationSpec( + joint_position=self._standing_position, + kp=self._preparation_kp, + kd=self._preparation_kd, 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.ready = JointServoPreparationState(robot) + self.ready.select(self._preparation_spec) self.damping = G1DampingController(self.policy_io._kd, robot) self.transitions: list[StageResult] = [] self._phase = "prepare" diff --git a/motrix_deploy_tasks/tests/test_cli.py b/motrix_deploy_tasks/tests/test_cli.py index 609eb63c..8e8659ef 100644 --- a/motrix_deploy_tasks/tests/test_cli.py +++ b/motrix_deploy_tasks/tests/test_cli.py @@ -30,6 +30,7 @@ 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.robot.g1_standing import resolve_g1_standing_gains from motrix_deploy_tasks.tasks.go2_walk import Go2WalkTaskSpec from motrix_env_core.config.scene import FlatTerrainCfg, HFieldTerrainCfg, SceneCfg @@ -95,6 +96,32 @@ def test_hardware_cli_help_selects_hardware_recipe() -> None: assert "backend: unitree" in result.stdout +def test_g1_hardware_recipe_composes_named_gains_and_operator_profile() -> None: + unitree_config = pytest.importorskip("motrix_deploy_unitree.config") + with initialize_config_module(version_base=None, config_module="motrix_deploy.config"): + cfg = compose( + config_name="deploy", + overrides=["task=g1-walk-flat/hardware", "runtime.network_interface=eth0"], + ) + runtime = OmegaConf.to_object(cfg.runtime) + + assert isinstance(runtime, unitree_config.UnitreeBackendConfig) + assert isinstance(runtime.robot, unitree_config.UnitreeG1_29DOFRobotCfg) + assert runtime.robot.mode_pr == 0 + assert runtime.robot.expected_mode_machine == 5 + assert runtime.robot.override_policy_gains is False + assert set(runtime.robot.kp) == set(runtime.robot.actuation.motors) + assert set(runtime.robot.kd) == set(runtime.robot.actuation.motors) + assert runtime.operator is not None + assert set(runtime.operator.intents) == { + "request_stand", + "enable_policy", + "enter_passive", + "emergency_stop", + } + assert runtime.operator.planar_velocity is not None + + @pytest.mark.parametrize( ("task", "runtime_kind", "backend"), [ @@ -246,6 +273,40 @@ def test_hardware_hydra_config_selects_runtime_defaults() -> None: assert cfg.runtime.robot.lie_down_button == "B" +def test_g1_29dof_hardware_recipe_selects_explicit_robot_contract() -> None: + unitree_config = pytest.importorskip("motrix_deploy_unitree.config") + unitree_hardware = pytest.importorskip("motrix_deploy_unitree.hardware") + with initialize_config_module(version_base=None, config_module="motrix_deploy.config"): + cfg = compose( + config_name="deploy", + overrides=[ + "task=g1-wbt-dance/hardware", + "runtime.network_interface=enp3s0", + ], + ) + + runtime = OmegaConf.to_object(cfg.runtime) + assert isinstance(runtime, unitree_config.UnitreeBackendConfig) + assert type(runtime.robot) is unitree_config.UnitreeG1_29DOFRobotCfg + assert runtime.robot.mode_pr == 0 + assert runtime.robot.expected_mode_machine == 5 + assert runtime.robot.motor_slot_count == 35 + assert runtime.robot.command_publish_period_s == pytest.approx(0.001) + assert cfg.artifact == "artifacts/g1-wbt-dance.deploy" + assert isinstance(runtime.robot, unitree_hardware.HardwareRobotCfg) + # FixStand gains are fixed in one place, not per recipe. + standing_kp, standing_kd = resolve_g1_standing_gains(tuple(runtime.robot.actuation.motors)) + np.testing.assert_array_equal( + np.asarray([runtime.robot.kp[name] for name in runtime.robot.actuation.motors], np.float32), standing_kp + ) + np.testing.assert_array_equal( + np.asarray([runtime.robot.kd[name] for name in runtime.robot.actuation.motors], np.float32), standing_kd + ) + assert runtime.network_interface == "enp3s0" + assert cfg.duration_s is not None and cfg.duration_s > 0 + assert "command" not in cfg + + def test_hardware_hydra_config_accepts_explicit_gain_overrides() -> None: pytest.importorskip("motrix_deploy_unitree.config") with initialize_config_module(version_base=None, config_module="motrix_deploy.config"): diff --git a/motrix_deploy_tasks/tests/test_g1_hot_swapping_program.py b/motrix_deploy_tasks/tests/test_g1_hot_swapping_program.py new file mode 100644 index 00000000..9ddaebdd --- /dev/null +++ b/motrix_deploy_tasks/tests/test_g1_hot_swapping_program.py @@ -0,0 +1,63 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +import numpy as np + +from motrix_deploy.artifact.io import read_artifact +from motrix_deploy.robot.g1_standing import resolve_g1_standing_gains +from motrix_deploy.runtime.hot_swapping import RobotHotSwappingController +from motrix_deploy.runtime.preparation import JointServoPreparationState +from motrix_deploy.runtime.state_machine import ImmediateTransition +from motrix_deploy_tasks.programs.g1_hot_swapping import G1HotSwappingProgramCfg, create_program + + +def _config(**overrides): + return G1HotSwappingProgramCfg( + walk_artifact="artifacts/g1-flat.deploy", + wbt_artifact="artifacts/g1-wbt-dance.deploy", + **overrides, + ) + + +def test_g1_program_fixes_fixstand_gains_but_keeps_preparation_per_artifact(): + program = create_program(_config()) + order = program.robot.joint_names + + standing_kp, standing_kd = resolve_g1_standing_gains(order) + fixstand = program.controller.states["fixstand"] + np.testing.assert_array_equal(fixstand._kp, standing_kp) + np.testing.assert_array_equal(fixstand._kd, standing_kd) + + # Preparation is a per-artifact stand-up recipe, not the fixed FixStand gains. + spec = read_artifact("artifacts/g1-wbt-dance.deploy").manifest.task + preparation = program.controller._skills["wbt"].preparation + np.testing.assert_array_equal(preparation.kp, np.asarray(spec.preparation_kp, np.float32)) + np.testing.assert_array_equal(preparation.kd, np.asarray(spec.preparation_kd, np.float32)) + np.testing.assert_array_equal(preparation.joint_position, fixstand._target) + assert preparation.ramp_duration_s == spec.preparation_ramp_duration_s + + +def test_g1_program_defaults_to_supported_simulation_startup_state(): + program = create_program(_config()) + assert program.controller.current_state == "fixstand" + assert isinstance(program.controller, RobotHotSwappingController) + assert isinstance(program.controller.states["skill_preparation"], JointServoPreparationState) + + +def test_g1_program_can_explicitly_use_passive_startup_for_hardware(): + program = create_program(_config(initial_state="passive")) + assert program.controller.current_state == "passive" + + +def test_g1_program_routes_walk_through_shared_preparation_and_returns_wbt_to_walk(): + program = create_program(_config()) + + assert ("fixstand", "wbt") not in program.controller._rules + assert isinstance(program.controller._rules[("walk", "skill_preparation")](), ImmediateTransition) + assert isinstance(program.controller._rules[("skill_preparation", "wbt")](), ImmediateTransition) + transition = program.controller._rules[("wbt", "walk")]() + assert isinstance(transition, ImmediateTransition) + program.controller._pending_skill = "wbt" + assert program.controller._completion_target("skill_preparation") == "wbt" + assert program.controller._completion_target("wbt") == "walk" + assert program.controller.pending_skill is None diff --git a/motrix_deploy_unitree/README.md b/motrix_deploy_unitree/README.md index 545b240e..26cdda8c 100644 --- a/motrix_deploy_unitree/README.md +++ b/motrix_deploy_unitree/README.md @@ -2,10 +2,16 @@ `motrix-deploy-unitree` provides the `unitree` SDK2 DDS hardware backend for `motrix-deploy`, asset-independent hardware sensor/motor configuration, and direct read/write and diagnostic tools. +G1 hardware recipes select the native C++ SDK2 transport (`transport: native_cpp`), while the +Python SDK transport remains available for compatibility diagnostics. The entry-point `create_runtime(config, context)` accepts a `UnitreeBackendConfig` with a nested -`UnitreeGo2RobotCfg` in `config.robot`, and returns a `HardwareRuntime`. Robot configurations inherit +`UnitreeGo2RobotCfg` or `UnitreeG1_29DOFRobotCfg` in `config.robot`, and returns a `HardwareRuntime`. Robot configurations inherit `HardwareRobotCfg` through `UnitreeRobotCfg`, exposing `key_pose`, `sensors`, and `actuation` directly -alongside robot-specific control settings. G1 hardware I/O is not implemented. The Unitree SDK is imported only when a hardware connection is opened. +alongside robot-specific control settings. G1 uses HG LowCmd/LowState, validates CRC plus +`mode_pr=0` and `mode_machine=5`, maps artifact joints to motors by name, and repeats the latest +policy command from a native C++ 1 kHz writer. Install the optional `native` extra to build the +`motrix-unitree-native` extension against Unitree SDK2; the extension is loaded only when a native +hardware connection is opened. ## Safety @@ -13,7 +19,8 @@ Use a suspended robot in low-level/debug mode with an operator-ready emergency s Opening a command interface may stand down the robot before the Start gate. Choose hardware-appropriate startup gains: Start transitions to the default pose, A enables control, Select triggers emergency stop, and B requests lie-down. Stop paths send damping commands. Even the single-joint helper publishes -commands for all 12 joints. The backend has fake-SDK test coverage; a real-robot smoke test remains pending. +commands for every configured joint. G1 unused HG slots remain disabled and zeroed. The backend has +fake-SDK test coverage; a suspended real-robot smoke test remains pending. ## Usage @@ -21,13 +28,28 @@ Check incoming state without creating a command publisher: ```bash motrix-deploy-unitree read-lowstate +motrix-deploy-unitree read-lowstate --robot g1-29dof ``` With `motrix-deploy-tasks` and a matching artifact installed: +Install the native transport extra before selecting a G1 hardware recipe: + +```bash +UNITREE_SDK2_ROOT=/opt/unitree_robotics uv sync --extra unitree +``` + +When installing the package outside this workspace, use the equivalent +`UNITREE_SDK2_ROOT=/opt/unitree_robotics python -m pip install +"motrix-deploy-unitree[native]"` command. The SDK2 root must contain `include/`, +`lib//`, and `thirdparty/lib//`. + ```bash mxlab deploy task=go2-walk-flat/hardware artifact=artifacts/go2-walk-flat.deploy \ runtime.network_interface= + +mxlab deploy task=g1-wbt-dance/hardware \ + runtime.network_interface= ``` Review configured startup gains and the diff --git a/motrix_deploy_unitree/pyproject.toml b/motrix_deploy_unitree/pyproject.toml index 914db245..0dd869e2 100644 --- a/motrix_deploy_unitree/pyproject.toml +++ b/motrix_deploy_unitree/pyproject.toml @@ -20,6 +20,9 @@ dependencies = [ "unitree-sdk2py", ] +[project.optional-dependencies] +native = ["motrix-unitree-native==0.4.0"] + [project.scripts] motrix-deploy-unitree = "motrix_deploy_unitree.cli:main" diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/__init__.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/__init__.py index b0d96704..81fb9c2c 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/__init__.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/__init__.py @@ -1,19 +1,26 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Unitree Go2 DDS hardware backend.""" +"""Unitree SDK2 DDS hardware backends.""" -__version__ = "0.3.0" +from importlib import metadata + +__version__ = metadata.version("motrix-deploy-unitree") from motrix_deploy_unitree.config import ( + G1_29DOF_JOINT_NAME_TO_MOTOR_ID, + G1_29DOF_MOTOR_COUNT, + G1_HG_MOTOR_SLOT_COUNT, GO2_JOINT_NAME_TO_MOTOR_INDEX, GO2_MOTOR_COUNT, UnitreeBackendConfig, + UnitreeG1_29DOFRobotCfg, UnitreeGo2RobotCfg, UnitreeRobotCfg, ) from motrix_deploy_unitree.direct import UnitreeGo2DirectInterface -from motrix_deploy_unitree.interface import UnitreeGo2Robot, UnitreeSdkBindings +from motrix_deploy_unitree.interface import UnitreeG1_29DOFRobot, UnitreeGo2Robot, UnitreeSdkBindings +from motrix_deploy_unitree.native_interface import UnitreeG1NativeRobot from motrix_deploy_unitree.remote import ( BUTTON_NAMES, UnitreeRemoteGamePadDevice, @@ -23,9 +30,15 @@ __all__ = [ "BUTTON_NAMES", + "G1_29DOF_JOINT_NAME_TO_MOTOR_ID", + "G1_29DOF_MOTOR_COUNT", + "G1_HG_MOTOR_SLOT_COUNT", "GO2_JOINT_NAME_TO_MOTOR_INDEX", "GO2_MOTOR_COUNT", "UnitreeBackendConfig", + "UnitreeG1_29DOFRobot", + "UnitreeG1NativeRobot", + "UnitreeG1_29DOFRobotCfg", "UnitreeGo2RobotCfg", "UnitreeRobotCfg", "UnitreeGo2DirectInterface", diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/cli.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/cli.py index 3f6aec9e..766b0733 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/cli.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/cli.py @@ -1,7 +1,7 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Command-line diagnostics for the Unitree Go2 deployment backend.""" +"""Command-line diagnostics for Unitree deployment backends.""" import argparse import math @@ -48,14 +48,15 @@ def _positive_int(value: str) -> int: def _parser() -> argparse.ArgumentParser: - parser = argparse.ArgumentParser(description="Unitree Go2 hardware diagnostics") + parser = argparse.ArgumentParser(description="Unitree hardware diagnostics") commands = parser.add_subparsers(dest="command", required=True) lowstate = commands.add_parser( "read-lowstate", help="Read rt/lowstate without creating a LowCmd publisher", ) - lowstate.add_argument("network_interface", help="Ethernet interface connected to Go2, for example enp5s0") + lowstate.add_argument("network_interface", help="Ethernet interface connected to the robot, for example enp5s0") + lowstate.add_argument("--robot", choices=("go2", "g1-29dof"), default="go2") lowstate.add_argument("--domain-id", type=_non_negative_int, default=0, help="DDS domain id (default: 0)") lowstate.add_argument("--topic", default="rt/lowstate", help="LowState DDS topic (default: rt/lowstate)") lowstate.add_argument("--queue-depth", type=_positive_int, default=10, help="Subscriber queue depth (default: 10)") @@ -98,6 +99,7 @@ def main(argv: Sequence[str] | None = None) -> int: duration_s=args.duration_s, print_interval_s=args.print_interval_s, validate_crc=not args.no_validate_crc, + robot=args.robot, ) run_joint_control( diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/config.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/config.py index 4a5b9261..a08c9a62 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/config.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/config.py @@ -14,17 +14,25 @@ from motrix_deploy.config import HardwareRuntimeCfg from motrix_deploy.errors import ValidationError +from motrix_deploy.operator import OperatorProfileCfg +from motrix_deploy.robot.g1_standing import G1_29DOF_STANDING_KD, G1_29DOF_STANDING_KP from motrix_deploy_unitree.actuation import Actuation -from motrix_deploy_unitree.hardware import HardwareRobotCfg, UnitreeGo2HardwareCfg -from motrix_deploy_unitree.remote import BUTTON_NAMES +from motrix_deploy_unitree.hardware import HardwareRobotCfg, UnitreeG1_29DOFHardwareCfg, UnitreeGo2HardwareCfg +from motrix_deploy_unitree.remote import ACCEPTED_BUTTON_NAMES, BUTTON_NAMES from motrix_deploy_unitree.sensor import HardwareSensorBindings from motrix_env_core.config.scene.base import KeyPoseCfg GO2_MOTOR_COUNT = 12 +G1_29DOF_MOTOR_COUNT = 29 +G1_HG_MOTOR_SLOT_COUNT = 35 _DEFAULT_GO2_ROBOT = UnitreeGo2HardwareCfg() +_DEFAULT_G1_29DOF_ROBOT = UnitreeG1_29DOFHardwareCfg() GO2_JOINT_NAME_TO_MOTOR_INDEX: Mapping[str, int] = MappingProxyType( {name: binding.index for name, binding in _DEFAULT_GO2_ROBOT.actuation.motors.items()} ) +G1_29DOF_JOINT_NAME_TO_MOTOR_ID: Mapping[str, int] = MappingProxyType( + {name: binding.index for name, binding in _DEFAULT_G1_29DOF_ROBOT.actuation.motors.items()} +) @dataclass @@ -41,19 +49,17 @@ class UnitreeRobotCfg(HardwareRobotCfg): @dataclass -class UnitreeGo2RobotCfg(UnitreeRobotCfg): - """Go2-specific hardware wiring, gains, and safety settings.""" - - key_pose: KeyPoseCfg = field(default_factory=lambda: UnitreeGo2HardwareCfg().key_pose) - sensors: HardwareSensorBindings = field(default_factory=lambda: UnitreeGo2HardwareCfg().sensors) - actuation: Actuation = field(default_factory=lambda: UnitreeGo2HardwareCfg().actuation) +class _UnitreeServoRobotCfg(UnitreeRobotCfg): + """Shared named-joint servo and safety configuration.""" + motor_slot_count: int = OC_MISSING joint_name_to_motor_index: dict[str, int] | None = None default_transition_duration_s: float = 2.0 damping_duration_s: float = 1.0 damping_kd: float = 8.0 kp: Any = None kd: Any = None + override_policy_gains: bool = True wait_for_remote_buttons: bool = True start_button: str = "start" enable_button: str = "A" @@ -62,8 +68,16 @@ class UnitreeGo2RobotCfg(UnitreeRobotCfg): lie_down_duration_s: float = 2.0 lie_down_joint_position: Any = None motor_mode: int | None = None + mode_pr: int | None = None + expected_mode_machine: int | None = None + command_publish_period_s: float | None = None + command_timeout_s: float = 0.1 + allow_lie_down: bool = True + startup_mode: str = "one_shot" def __post_init__(self) -> None: + if self.startup_mode not in {"one_shot", "state_machine"}: + raise ValidationError("runtime.robot.startup_mode", "one_shot or state_machine", self.startup_mode) if not isinstance(self.wait_for_remote_buttons, bool): raise ValidationError("runtime.robot.wait_for_remote_buttons", "a boolean", self.wait_for_remote_buttons) for name in ( @@ -75,6 +89,16 @@ def __post_init__(self) -> None: value = getattr(self, name) if not isinstance(value, (int, float)) or isinstance(value, bool) or not np.isfinite(value): raise ValidationError(f"runtime.robot.{name}", "a finite number", value) + if ( + not isinstance(self.motor_slot_count, int) + or isinstance(self.motor_slot_count, bool) + or self.motor_slot_count <= 0 + ): + raise ValidationError("runtime.robot.motor_slot_count", "a positive integer", self.motor_slot_count) + if not isinstance(self.allow_lie_down, bool): + raise ValidationError("runtime.robot.allow_lie_down", "a boolean", self.allow_lie_down) + if not isinstance(self.override_policy_gains, bool): + raise ValidationError("runtime.robot.override_policy_gains", "a boolean", self.override_policy_gains) if self.default_transition_duration_s <= 0: raise ValidationError( "runtime.robot.default_transition_duration_s", "a positive duration", self.default_transition_duration_s @@ -93,6 +117,30 @@ def __post_init__(self) -> None: or not 0 <= self.motor_mode <= 0xFF ): raise ValidationError("runtime.robot.motor_mode", "an integer in [0, 255]", self.motor_mode) + for name in ("mode_pr", "expected_mode_machine"): + value = getattr(self, name) + if value is not None and (not isinstance(value, int) or isinstance(value, bool) or not 0 <= value <= 0xFF): + raise ValidationError(f"runtime.robot.{name}", "an integer in [0, 255]", value) + if self.command_publish_period_s is not None and ( + not isinstance(self.command_publish_period_s, (int, float)) + or isinstance(self.command_publish_period_s, bool) + or not np.isfinite(self.command_publish_period_s) + or self.command_publish_period_s <= 0 + ): + raise ValidationError( + "runtime.robot.command_publish_period_s", + "a positive finite duration or null", + self.command_publish_period_s, + ) + if ( + not isinstance(self.command_timeout_s, (int, float)) + or isinstance(self.command_timeout_s, bool) + or not np.isfinite(self.command_timeout_s) + or self.command_timeout_s <= 0 + ): + raise ValidationError( + "runtime.robot.command_timeout_s", "a positive finite duration", self.command_timeout_s + ) for name in ("start_button", "enable_button", "emergency_stop_button", "lie_down_button"): value = getattr(self, name) if value not in BUTTON_NAMES: @@ -107,20 +155,37 @@ def __post_init__(self) -> None: type(self.joint_name_to_motor_index).__name__, ) - @staticmethod - def _validate_gain_override(name: str, value: object | None) -> None: + def _validate_gain_override(self, name: str, value: object | None) -> None: if value is None: return + if isinstance(value, Mapping): + expected = set(self.actuation.motors) + actual = set(value) + if actual != expected: + raise ValidationError( + f"runtime.robot.{name}", + f"exactly joint names {sorted(expected)}", + f"missing={sorted(expected - actual)}, unknown={sorted(actual - expected)}", + ) + if any( + not isinstance(item, (int, float)) or isinstance(item, bool) or not np.isfinite(item) or item < 0 + for item in value.values() + ): + raise ValidationError(f"runtime.robot.{name}", "finite non-negative named gains", value) + return if isinstance(value, bool): - raise ValidationError(f"runtime.robot.{name}", "a non-negative scalar or 12 values", value) + raise ValidationError(f"runtime.robot.{name}", "a non-negative scalar or joint vector", value) try: array = np.asarray(value, dtype=np.float32) except (TypeError, ValueError) as error: - raise ValidationError(f"runtime.robot.{name}", "a non-negative scalar or 12 values", value) from error - if array.shape not in ((), (GO2_MOTOR_COUNT,)) or not np.all(np.isfinite(array)) or np.any(array < 0): + raise ValidationError(f"runtime.robot.{name}", "a non-negative scalar or joint vector", value) from error + joint_count = len(self.actuation.motors) + if array.shape not in ((), (joint_count,)): raise ValidationError( - f"runtime.robot.{name}", "a non-negative scalar or 12 finite non-negative values", value + f"runtime.robot.{name}", f"a non-negative scalar or {joint_count} joint values", value ) + if not np.all(np.isfinite(array)) or np.any(array < 0): + raise ValidationError(f"runtime.robot.{name}", "finite non-negative gains", value) def lie_down_position(self) -> np.ndarray: """Return the named shutdown pose or an explicit canonical override.""" @@ -130,13 +195,13 @@ def lie_down_position(self) -> np.ndarray: except (TypeError, ValueError) as error: raise ValidationError( "runtime.robot.lie_down_joint_position", - "12 finite canonical joint positions", + f"{len(self.actuation.motors)} finite canonical joint positions", self.lie_down_joint_position, ) from error - if position.shape != (GO2_MOTOR_COUNT,) or not np.all(np.isfinite(position)): + if position.shape != (len(self.actuation.motors),) or not np.all(np.isfinite(position)): raise ValidationError( "runtime.robot.lie_down_joint_position", - "12 finite canonical joint positions", + f"{len(self.actuation.motors)} finite canonical joint positions", self.lie_down_joint_position, ) return position.copy() @@ -146,15 +211,25 @@ def lie_down_position(self) -> np.ndarray: raise ValidationError("runtime.robot.key_pose", "a named lie_down pose", "missing") from error return np.asarray(pose, dtype=np.float32).copy() - def gain_override(self, name: str, joint_count: int) -> np.ndarray | None: + def gain_override(self, name: str, joint_names: tuple[str, ...]) -> np.ndarray | None: value = getattr(self, name) if value is None: return None + if isinstance(value, Mapping): + expected = set(joint_names) + actual = set(value) + if actual != expected: + raise ValidationError( + f"runtime.robot.{name}", + f"exactly artifact joints {sorted(expected)}", + f"missing={sorted(expected - actual)}, unknown={sorted(actual - expected)}", + ) + return np.asarray([value[joint] for joint in joint_names], dtype=np.float32) array = np.asarray(value, dtype=np.float32) if array.shape == (): - return np.full(joint_count, array.item(), dtype=np.float32) - if array.shape != (joint_count,): - raise ValidationError(f"runtime.robot.{name}", f"{joint_count} canonical joint gains", value) + return np.full(len(joint_names), array.item(), dtype=np.float32) + if array.shape != (len(joint_names),): + raise ValidationError(f"runtime.robot.{name}", f"{len(joint_names)} canonical joint gains", value) return np.array(array, dtype=np.float32, copy=True) def motor_indices(self, joint_names: tuple[str, ...]) -> np.ndarray: @@ -172,16 +247,54 @@ def motor_indices(self, joint_names: tuple[str, ...]) -> np.ndarray: values = tuple(mapping[name] for name in joint_names) if any(not isinstance(index, int) or isinstance(index, bool) for index in values): raise ValidationError("runtime.robot.motor_indices", "integer motor indices", values) - if len(set(values)) != len(values) or any(not 0 <= index < GO2_MOTOR_COUNT for index in values): - raise ValidationError("runtime.robot.motor_indices", f"unique values in [0, {GO2_MOTOR_COUNT - 1}]", values) + if len(set(values)) != len(values) or any(not 0 <= index < self.motor_slot_count for index in values): + raise ValidationError( + "runtime.robot.motor_indices", f"unique values in [0, {self.motor_slot_count - 1}]", values + ) return np.asarray(values, dtype=np.int64) +@dataclass +class UnitreeGo2RobotCfg(_UnitreeServoRobotCfg): + """Go2-specific hardware wiring, gains, and safety settings.""" + + key_pose: KeyPoseCfg = field(default_factory=lambda: UnitreeGo2HardwareCfg().key_pose) + sensors: HardwareSensorBindings = field(default_factory=lambda: UnitreeGo2HardwareCfg().sensors) + actuation: Actuation = field(default_factory=lambda: UnitreeGo2HardwareCfg().actuation) + motor_slot_count: int = GO2_MOTOR_COUNT + + +@dataclass +class UnitreeG1_29DOFRobotCfg(_UnitreeServoRobotCfg): + """G1 29-DOF HG wiring, mode contract, and high-rate publisher settings.""" + + key_pose: KeyPoseCfg = field(default_factory=lambda: UnitreeG1_29DOFHardwareCfg().key_pose) + sensors: HardwareSensorBindings = field(default_factory=lambda: UnitreeG1_29DOFHardwareCfg().sensors) + actuation: Actuation = field(default_factory=lambda: UnitreeG1_29DOFHardwareCfg().actuation) + motor_slot_count: int = G1_HG_MOTOR_SLOT_COUNT + # Fixed FixStand gains, shared with the program and task deployment paths. + kp: Any = field(default_factory=lambda: dict(G1_29DOF_STANDING_KP)) + kd: Any = field(default_factory=lambda: dict(G1_29DOF_STANDING_KD)) + mode_pr: int | None = 0 + expected_mode_machine: int | None = 5 + command_publish_period_s: float | None = 0.001 + command_timeout_s: float = 0.1 + default_transition_duration_s: float = 3.0 + damping_kd: float = 3.0 + motor_mode: int | None = 1 + allow_lie_down: bool = False + override_policy_gains: bool = False + + @dataclass class UnitreeBackendConfig(HardwareRuntimeCfg): """Shared Unitree DDS transport configuration and selected robot recipe.""" backend: str = "unitree" + # Keep the Python SDK as the explicit compatibility default for existing + # injected-transport tests and diagnostics. Hardware recipes that use the + # native real-time path set ``transport: native_cpp``. + transport: str = "python_sdk" network_interface: str = OC_MISSING domain_id: int = 0 lowcmd_topic: str = "rt/lowcmd" @@ -189,9 +302,12 @@ class UnitreeBackendConfig(HardwareRuntimeCfg): connect_timeout_s: float = 5.0 validate_crc: bool = True subscriber_queue_depth: int = 10 + operator: OperatorProfileCfg | None = None robot: UnitreeRobotCfg = OC_MISSING def __post_init__(self) -> None: + if self.transport not in {"python_sdk", "native_cpp"}: + raise ValidationError("runtime.transport", "python_sdk or native_cpp", self.transport) if not isinstance(self.network_interface, str) or not self.network_interface: raise ValidationError("runtime.network_interface", "a non-empty interface name", self.network_interface) if not isinstance(self.validate_crc, bool): @@ -217,6 +333,43 @@ def __post_init__(self) -> None: raise ValidationError("runtime.subscriber_queue_depth", "a positive integer", self.subscriber_queue_depth) if not isinstance(self.robot, UnitreeRobotCfg): raise ValidationError("runtime.robot", "a UnitreeRobotCfg config", type(self.robot).__name__) + if self.operator is not None: + if not isinstance(self.operator, OperatorProfileCfg): + raise ValidationError("runtime.operator", "an OperatorProfileCfg", type(self.operator).__name__) + self._validate_operator_profile() + + def _validate_operator_profile(self) -> None: + assert self.operator is not None + for intent, gesture in self.operator.intents.items(): + buttons = [*gesture.held, *gesture.held_for_s] + buttons.extend(name for name in (gesture.pressed, gesture.released) if name is not None) + unknown = sorted(set(buttons) - set(ACCEPTED_BUTTON_NAMES)) + if unknown: + raise ValidationError( + f"runtime.operator.intents.{intent}", + f"buttons in {sorted(ACCEPTED_BUTTON_NAMES)}", + unknown, + ) + velocity = self.operator.planar_velocity + if velocity is not None: + axes = {velocity.linear_x_axis, velocity.linear_y_axis, velocity.yaw_axis} + if not axes <= {"lx", "ly", "rx", "ry"}: + raise ValidationError("runtime.operator.planar_velocity.axes", "Unitree joystick axes", sorted(axes)) + if velocity.deadman_button is not None and velocity.deadman_button not in ACCEPTED_BUTTON_NAMES: + raise ValidationError( + "runtime.operator.planar_velocity.deadman_button", + f"one of {sorted(ACCEPTED_BUTTON_NAMES)}", + velocity.deadman_button, + ) + if isinstance(self.robot, UnitreeG1_29DOFRobotCfg): + required = ( + {"request_stand", "enable_policy", "enter_passive", "emergency_stop"} + if self.robot.startup_mode == "one_shot" + else {"request_fixstand", "request_walk", "request_wbt", "request_passive", "request_emergency"} + ) + missing = sorted(required - set(self.operator.intents)) + if missing: + raise ValidationError("runtime.operator.intents", f"G1 safety intents {sorted(required)}", missing) def install_hardware_runtime_schema() -> None: @@ -224,12 +377,17 @@ def install_hardware_runtime_schema() -> None: store = ConfigStore.instance() store.store(group="runtime", name="_unitree_schema", node=UnitreeBackendConfig) store.store(group="unitree_robot", name="go2", node=UnitreeGo2RobotCfg) + store.store(group="unitree_robot", name="g1-29dof", node=UnitreeG1_29DOFRobotCfg) __all__ = [ "GO2_JOINT_NAME_TO_MOTOR_INDEX", "GO2_MOTOR_COUNT", + "G1_29DOF_MOTOR_COUNT", + "G1_29DOF_JOINT_NAME_TO_MOTOR_ID", + "G1_HG_MOTOR_SLOT_COUNT", "UnitreeBackendConfig", + "UnitreeG1_29DOFRobotCfg", "UnitreeGo2RobotCfg", "UnitreeRobotCfg", "install_hardware_runtime_schema", diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/hardware.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/hardware.py index f93d1c83..ab412d93 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/hardware.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/hardware.py @@ -86,3 +86,107 @@ class UnitreeGo2HardwareCfg(HardwareRobotCfg): "RR_calf_joint": UnitreeMotorBinding(index=8), } ) + + +G1_29DOF_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", +) + +_G1_29DOF_DEFAULT_POSITION = ( + -0.312, + 0.0, + 0.0, + 0.669, + -0.363, + 0.0, + -0.312, + 0.0, + 0.0, + 0.669, + -0.363, + 0.0, + 0.0, + 0.0, + 0.0, + 0.2, + 0.2, + 0.0, + 0.6, + 0.0, + 0.0, + 0.0, + 0.2, + -0.2, + 0.0, + 0.6, + 0.0, + 0.0, + 0.0, +) + + +@configclass(kw_only=True) +class UnitreeG1_29DOFHardwareCfg(HardwareRobotCfg): + """G1 29-DOF wiring for the HG DDS protocol. + + Joint names are the stable semantic contract. Motor ids deliberately live + here rather than in a policy-order permutation so exported artifacts may + use any joint ordering without changing the physical wiring. + """ + + key_pose: KeyPoseCfg = KeyPoseCfg( + joint_names=list(G1_29DOF_JOINT_NAMES), + poses={ + "default": list(_G1_29DOF_DEFAULT_POSITION), + # G1 shutdown is damping-only. This pose only satisfies the shared + # pose schema and is never selected by the G1 adapter. + "lie_down": list(_G1_29DOF_DEFAULT_POSITION), + }, + ) + sensors: HardwareSensorBindings = HardwareSensorBindings( + base_orientation_xyzw=HardwareSensorBinding(field="imu_state.quaternion", quaternion_order="wxyz"), + base_angular_velocity=HardwareSensorBinding(field="imu_state.gyroscope"), + base_linear_acceleration=HardwareSensorBinding(field="imu_state.accelerometer"), + ) + actuation: Actuation = Actuation( + motors={ + name: UnitreeMotorBinding(index=motor_id, enabled_mode=1) + for motor_id, name in enumerate(G1_29DOF_JOINT_NAMES) + } + ) + + +__all__ = [ + "G1_29DOF_JOINT_NAMES", + "HardwareRobotCfg", + "UnitreeG1_29DOFHardwareCfg", + "UnitreeGo2HardwareCfg", +] diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/interface.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/interface.py index 48388ed8..290d1b94 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/interface.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/interface.py @@ -24,9 +24,19 @@ RobotSpec, RobotState, ) -from motrix_deploy.errors import EmergencyStopError, LieDownRequestedError, ValidationError +from motrix_deploy.errors import ( + EmergencyStopError, + LieDownRequestedError, + PassiveRequestedError, + ValidationError, +) +from motrix_deploy.operator import OperatorIntentEvaluator from motrix_deploy.robot.interface import RobotInterface -from motrix_deploy_unitree.config import GO2_MOTOR_COUNT, UnitreeBackendConfig, UnitreeGo2RobotCfg +from motrix_deploy_unitree.config import ( + UnitreeBackendConfig, + UnitreeG1_29DOFRobotCfg, + UnitreeGo2RobotCfg, +) from motrix_deploy_unitree.remote import UnitreeRemoteGamePadDevice, UnitreeRemoteState, decode_wireless_remote from motrix_env_core.input import GamePadDevice @@ -86,9 +96,37 @@ def _load_sdk_bindings() -> UnitreeSdkBindings: ) +def _load_g1_29dof_sdk_bindings() -> UnitreeSdkBindings: + """Load the HG messages used by a physical G1 29-DOF robot.""" + try: + from unitree_sdk2py.core.channel import ChannelFactoryInitialize, ChannelPublisher, ChannelSubscriber + from unitree_sdk2py.idl.default import unitree_hg_msg_dds__LowCmd_ + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as LowCmdHg + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ as LowStateHg + from unitree_sdk2py.utils.crc import CRC + except ImportError as error: + raise RuntimeError( + "Unitree G1 29-DOF support requires the Motphys unitree_sdk2py package; " + "install https://github.com/motphys-developers/unitree_sdk2_python" + ) from error + return UnitreeSdkBindings( + channel_factory_initialize=ChannelFactoryInitialize, + channel_publisher=ChannelPublisher, + channel_subscriber=ChannelSubscriber, + low_cmd_message_type=LowCmdHg, + low_state_message_type=LowStateHg, + make_low_cmd=unitree_hg_msg_dds__LowCmd_, + crc_type=CRC, + ) + + class UnitreeGo2Robot(RobotInterface): """Map canonical Go2 state/commands to SDK2 LowState/LowCmd DDS messages.""" + robot_config_type = UnitreeGo2RobotCfg + disabled_position = _POSITION_STOP + disabled_velocity = _VELOCITY_STOP + def __init__( self, config: UnitreeBackendConfig, @@ -100,8 +138,8 @@ def __init__( clock_ns: Callable[[], int] = time.monotonic_ns, sleep: Callable[[float], None] = time.sleep, ) -> None: - if not isinstance(config.robot, UnitreeGo2RobotCfg): - raise TypeError("UnitreeGo2Robot requires UnitreeGo2RobotCfg") + if not isinstance(config.robot, self.robot_config_type): + raise TypeError(f"{type(self).__name__} requires {self.robot_config_type.__name__}") self.config = config for path, value in ( ("control.period_s", control_period_s), @@ -117,6 +155,9 @@ def __init__( self._sleep = sleep self._condition = threading.Condition() self._write_lock = threading.Lock() + self._writer_stop = threading.Event() + self._writer_thread: threading.Thread | None = None + self._latest_command_update_ns = 0 self._publisher: Any = None self._subscriber: Any = None self._low_cmd: Any = None @@ -136,8 +177,11 @@ def __init__( self._last_read_sequence = 0 self._remote = UnitreeRemoteState() self._gamepad = UnitreeRemoteGamePadDevice(lambda: self.remote) + self._intent_evaluator = None if config.operator is None else OperatorIntentEvaluator(config.operator.intents) + self._latched_intents: set[str] = set() self._callback_error: Exception | None = None self._emergency_stop = False + self._passive_requested = False self._lie_down_requested = False self._last_position_kp = np.empty(0, dtype=np.float32) self._last_position_kd = np.empty(0, dtype=np.float32) @@ -148,6 +192,9 @@ def __init__( self._stopped = False self._closed = False + def _load_sdk_bindings(self) -> UnitreeSdkBindings: + return _load_sdk_bindings() + @property def spec(self) -> RobotSpec: return self._spec @@ -204,9 +251,9 @@ def open(self) -> None: if self._opened and not self._closed: raise RuntimeError("Unitree backend is already open") self._resolve_hardware_bindings(self._spec) - self._kp_override = self.config.robot.gain_override("kp", self._spec.joint_count) - self._kd_override = self.config.robot.gain_override("kd", self._spec.joint_count) - sdk = self._sdk or _load_sdk_bindings() + self._kp_override = self.config.robot.gain_override("kp", self._spec.joint_names) + self._kd_override = self.config.robot.gain_override("kd", self._spec.joint_names) + sdk = self._sdk or self._load_sdk_bindings() self._sdk = sdk try: sdk.channel_factory_initialize(self.config.domain_id, self.config.network_interface) @@ -226,24 +273,27 @@ def open(self) -> None: self._low_cmd = sdk.make_low_cmd() self._initialize_low_cmd() - self._publisher = sdk.channel_publisher(self.config.lowcmd_topic, sdk.low_cmd_message_type) - self._publisher.Init() self._subscriber = sdk.channel_subscriber(self.config.lowstate_topic, sdk.low_state_message_type) self._subscriber.Init(self._low_state_handler, self.config.subscriber_queue_depth) self._wait_for_first_state() assert self._latest_low_state is not None - if len(self._latest_low_state.motor_state) < GO2_MOTOR_COUNT: + motor_slot_count = self.config.robot.motor_slot_count + if len(self._latest_low_state.motor_state) < motor_slot_count: raise ValidationError( "backend.lowstate.motor_state", - f"at least {GO2_MOTOR_COUNT} motors", + f"at least {motor_slot_count} motors", len(self._latest_low_state.motor_state), ) - if len(self._low_cmd.motor_cmd) < GO2_MOTOR_COUNT: + if len(self._low_cmd.motor_cmd) < motor_slot_count: raise ValidationError( "backend.lowcmd.motor_cmd", - f"at least {GO2_MOTOR_COUNT} motors", + f"at least {motor_slot_count} motors", len(self._low_cmd.motor_cmd), ) + # A LowCmd publisher is created only after a CRC-valid LowState has + # passed the robot-specific mode and motor-slot contract. + self._publisher = sdk.channel_publisher(self.config.lowcmd_topic, sdk.low_cmd_message_type) + self._publisher.Init() self._capabilities = RobotCapabilities( control_modes=(JointControlMode.SERVO, JointControlMode.TORQUE), state_fields=frozenset( @@ -266,6 +316,10 @@ def open(self) -> None: self._closed = False self._health_reason = "" self._last_communication_ns = self._latest_receive_time_ns + if self.config.robot.command_publish_period_s is not None: + # Queue the robot-specific passive frame before the periodic writer starts. + self._publish_pre_enable_command() + self._start_command_writer() def _resolve_hardware_bindings(self, spec: RobotSpec) -> None: robot = self.config.robot @@ -299,6 +353,11 @@ def enable(self) -> None: self._require_open() if self._enabled: return + if self.config.robot.startup_mode == "state_machine": + # The periodic writer already owns a robot-specific passive frame. + # The persistent controller performs every posture/policy transition. + self._enabled = True + return if self._kp_override is None or self._kd_override is None: raise ValidationError( "backend.enable", @@ -318,7 +377,10 @@ def enable(self) -> None: self._last_position_kp = np.array(kp, dtype=np.float32, copy=True) self._last_position_kd = np.array(kd, dtype=np.float32, copy=True) if self.config.robot.wait_for_remote_buttons: - self._wait_for_button(self.config.robot.start_button, self._publish_zero_torque) + if self._intent_evaluator is None: + self._wait_for_button(self.config.robot.start_button, self._publish_pre_enable_command) + else: + self._wait_for_intent("request_stand", self._publish_pre_enable_command) start = self._latest_robot_state().joint_position steps = max(1, math.ceil(self.config.robot.default_transition_duration_s / self._control_period_s)) for step in range(1, steps + 1): @@ -335,10 +397,14 @@ def enable(self) -> None: self._wait_control_period() if self.config.robot.wait_for_remote_buttons: - self._wait_for_button( - self.config.robot.enable_button, - lambda: self._publish_robot_command(initial_command), - ) + + def publish_stand() -> None: + self._publish_robot_command(initial_command) + + if self._intent_evaluator is None: + self._wait_for_button(self.config.robot.enable_button, publish_stand) + else: + self._wait_for_intent("enable_policy", publish_stand) self._raise_if_remote_stop_requested() self._enabled = True @@ -372,10 +438,13 @@ def write_command(self, command: RobotCommand) -> None: def health(self) -> HealthStatus: with self._condition: emergency = self._emergency_stop + passive = self._passive_requested lie_down = self._lie_down_requested reason = self._health_reason if emergency: reason = f"remote {self.config.robot.emergency_stop_button!r} emergency stop is active" + elif passive: + reason = "operator requested passive damping" elif lie_down and not reason: reason = f"remote {self.config.robot.lie_down_button!r} lie-down shutdown is active" elif self._closed: @@ -395,7 +464,7 @@ def stop(self) -> None: return self._enabled = False with self._condition: - run_lie_down = self._lie_down_requested and not self._emergency_stop + run_lie_down = self.config.robot.allow_lie_down and self._lie_down_requested and not self._emergency_stop if run_lie_down: try: self._publish_lie_down_trajectory() @@ -421,6 +490,10 @@ def close(self) -> None: self.stop() except Exception as error: errors.append(error) + try: + self._stop_command_writer() + except Exception as error: + errors.append(error) for channel in (self._subscriber, self._publisher): if channel is None: continue @@ -452,9 +525,20 @@ def _low_state_handler(self, message: Any) -> None: self._latest_receive_time_ns = receive_time_ns self._state_sequence += 1 self._remote = remote - if remote.pressed(self.config.robot.emergency_stop_button): + if self._intent_evaluator is not None: + intents = self._intent_evaluator.update(remote.pressed_buttons(), receive_time_ns) + self._latched_intents.update(intents) + if "emergency_stop" in intents or "request_emergency" in intents: + self._emergency_stop = True + if "enter_passive" in intents and self.config.robot.startup_mode == "one_shot": + self._passive_requested = True + # Keep select/back as an unconditional, non-configurable fallback + # emergency stop in addition to the configured semantic gesture. + if remote.pressed("select") or ( + self._intent_evaluator is None and remote.pressed(self.config.robot.emergency_stop_button) + ): self._emergency_stop = True - if remote.pressed(self.config.robot.lie_down_button): + if self.config.robot.allow_lie_down and remote.pressed(self.config.robot.lie_down_button): self._lie_down_requested = True self._condition.notify_all() @@ -478,6 +562,16 @@ def _wait_for_button(self, button: str, publish: Callable[[], None]) -> None: publish() self._wait_control_period() + def _wait_for_intent(self, intent: str, publish: Callable[[], None]) -> None: + while True: + with self._condition: + if intent in self._latched_intents: + self._latched_intents.remove(intent) + return + self._raise_if_remote_stop_requested() + publish() + self._wait_control_period() + def _wait_control_period(self) -> None: self._sleep(self._control_period_s) with self._condition: @@ -517,12 +611,23 @@ def _state_from_message(self, message: Any, receive_time_ns: int) -> RobotState: ) def _validate_message_fields(self, message: Any) -> None: - if len(message.motor_state) < GO2_MOTOR_COUNT: + motor_slot_count = self.config.robot.motor_slot_count + if len(message.motor_state) < motor_slot_count: raise ValidationError( "state.motor_state", - f"at least {GO2_MOTOR_COUNT} motor states", + f"at least {motor_slot_count} motor states", len(message.motor_state), ) + expected_mode_pr = self.config.robot.mode_pr + if expected_mode_pr is not None: + actual_mode_pr = getattr(message, "mode_pr", None) + if actual_mode_pr != expected_mode_pr: + raise ValidationError("state.mode_pr", expected_mode_pr, actual_mode_pr) + expected_mode_machine = self.config.robot.expected_mode_machine + if expected_mode_machine is not None: + actual_mode_machine = getattr(message, "mode_machine", None) + if actual_mode_machine != expected_mode_machine: + raise ValidationError("state.mode_machine", expected_mode_machine, actual_mode_machine) for role, expected in ( ("base_orientation_xyzw", 4), ("base_angular_velocity", 3), @@ -565,14 +670,19 @@ def _validate_command(self, command: RobotCommand) -> None: ) def _initialize_low_cmd(self) -> None: - self._low_cmd.head[0] = 0xFE - self._low_cmd.head[1] = 0xEF - self._low_cmd.level_flag = 0xFF - self._low_cmd.gpio = 0 + if hasattr(self._low_cmd, "head"): + self._low_cmd.head[0] = 0xFE + self._low_cmd.head[1] = 0xEF + self._low_cmd.level_flag = 0xFF + self._low_cmd.gpio = 0 + if self.config.robot.mode_pr is not None: + self._low_cmd.mode_pr = self.config.robot.mode_pr + if self.config.robot.expected_mode_machine is not None: + self._low_cmd.mode_machine = self.config.robot.expected_mode_machine for motor in self._low_cmd.motor_cmd: motor.mode = 0 - motor.q = _POSITION_STOP - motor.qd = _VELOCITY_STOP + motor.q = self.disabled_position + motor.dq = self.disabled_velocity motor.kp = 0.0 motor.kd = 0.0 motor.tau = 0.0 @@ -587,7 +697,7 @@ def _publish_robot_command(self, command: RobotCommand) -> None: motor = self._low_cmd.motor_cmd[int(motor_index)] motor.mode = self._motor_modes[canonical_index] motor.q = 0.0 - motor.qd = 0.0 + motor.dq = 0.0 motor.kp = 0.0 motor.kd = 0.0 motor.tau = float(command.torque[canonical_index]) @@ -603,6 +713,8 @@ def _publish_robot_command(self, command: RobotCommand) -> None: ) def _effective_gains(self, command: JointServoCommand) -> tuple[np.ndarray, np.ndarray]: + if self._enabled and not self.config.robot.override_policy_gains: + return command.kp, command.kd kp = command.kp if self._kp_override is None else self._kp_override kd = command.kd if self._kd_override is None else self._kd_override return kp, kd @@ -622,7 +734,7 @@ def _publish_joint_fields( motor = self._low_cmd.motor_cmd[int(motor_index)] motor.mode = self._motor_modes[canonical_index] motor.q = float(position[canonical_index]) - motor.qd = float(velocity[canonical_index]) + motor.dq = float(velocity[canonical_index]) motor.kp = float(kp[canonical_index]) motor.kd = float(kd[canonical_index]) motor.tau = float(torque[canonical_index]) @@ -634,18 +746,22 @@ def _publish_zero_torque(self) -> None: motor = self._low_cmd.motor_cmd[int(motor_index)] motor.mode = self._motor_modes[canonical_index] motor.q = 0.0 - motor.qd = 0.0 + motor.dq = 0.0 motor.kp = 0.0 motor.kd = 0.0 motor.tau = 0.0 self._write_low_cmd_locked() + def _publish_pre_enable_command(self) -> None: + self._publish_zero_torque() + def _publish_lie_down_trajectory(self) -> None: - if self._last_position_kp.shape != (GO2_MOTOR_COUNT,) or self._last_position_kd.shape != (GO2_MOTOR_COUNT,): + joint_count = self._spec.joint_count + if self._last_position_kp.shape != (joint_count,) or self._last_position_kd.shape != (joint_count,): raise RuntimeError("position gains are unavailable for lie-down shutdown") start = self._latest_robot_state().joint_position target = self._lie_down_position - zeros = np.zeros(GO2_MOTOR_COUNT, dtype=np.float32) + zeros = np.zeros(joint_count, dtype=np.float32) steps = max(1, math.ceil(self.config.robot.lie_down_duration_s / self._control_period_s)) for step in range(1, steps + 1): with self._condition: @@ -668,21 +784,87 @@ def _publish_lie_down_trajectory(self) -> None: def _publish_damping(self) -> None: with self._write_lock: - for canonical_index, motor_index in enumerate(self._motor_indices): - motor = self._low_cmd.motor_cmd[int(motor_index)] - motor.mode = self._motor_modes[canonical_index] - motor.q = 0.0 - motor.qd = 0.0 - motor.kp = 0.0 - motor.kd = self.config.robot.damping_kd - motor.tau = 0.0 + self._set_damping_fields_locked() self._write_low_cmd_locked() + def _set_damping_fields_locked(self) -> None: + state = self._latest_low_state + for canonical_index, motor_index in enumerate(self._motor_indices): + motor = self._low_cmd.motor_cmd[int(motor_index)] + motor.mode = self._motor_modes[canonical_index] + motor.q = 0.0 if state is None else float(state.motor_state[int(motor_index)].q) + motor.dq = 0.0 + motor.kp = 0.0 + motor.kd = self.config.robot.damping_kd + motor.tau = 0.0 + def _write_low_cmd_locked(self) -> None: + self._latest_command_update_ns = self._clock_ns() + if self.config.robot.command_publish_period_s is not None: + return + self._publish_low_cmd_locked() + + def _publish_low_cmd_locked(self) -> None: self._low_cmd.crc = self._command_crc.Crc(self._low_cmd) self._publisher.Write(self._low_cmd) self._last_communication_ns = self._clock_ns() + def _start_command_writer(self) -> None: + if self._writer_thread is not None: + raise RuntimeError("Unitree command writer is already running") + self._writer_stop.clear() + self._writer_thread = threading.Thread( + target=self._command_writer_loop, + name=f"{type(self).__name__}-lowcmd", + daemon=True, + ) + self._writer_thread.start() + + def _stop_command_writer(self) -> None: + thread = self._writer_thread + if thread is None: + return + if self._publisher is not None and self._low_cmd is not None and self._command_crc is not None: + # stop() has already replaced the buffer with damping. Publish it + # synchronously once so shutdown does not depend on thread timing. + with self._write_lock: + self._publish_low_cmd_locked() + self._writer_stop.set() + thread.join(timeout=1.0) + if thread.is_alive(): + raise RuntimeError("Unitree command writer did not stop") + self._writer_thread = None + + def _command_writer_loop(self) -> None: + period_s = self.config.robot.command_publish_period_s + assert period_s is not None + period_ns = max(1, round(period_s * 1e9)) + command_timeout_ns = round(self.config.robot.command_timeout_s * 1e9) + state_timeout_ns = round(self._state_timeout_s * 1e9) + next_deadline_ns = self._clock_ns() + try: + while not self._writer_stop.is_set(): + now_ns = self._clock_ns() + with self._write_lock: + state_stale = now_ns - self._latest_receive_time_ns > state_timeout_ns + command_stale = now_ns - self._latest_command_update_ns > command_timeout_ns + if state_stale or command_stale: + self._set_damping_fields_locked() + stale = "LowState" if state_stale else "policy command" + self._health_reason = f"{stale} is stale; publishing damping" + self._publish_low_cmd_locked() + next_deadline_ns += period_ns + remaining_s = (next_deadline_ns - self._clock_ns()) / 1e9 + if remaining_s <= 0: + next_deadline_ns = self._clock_ns() + continue + self._writer_stop.wait(remaining_s) + except Exception as error: + with self._condition: + self._callback_error = error + self._health_reason = f"LowCmd writer failed: {error}" + self._condition.notify_all() + def _raise_if_remote_stop_requested(self) -> None: with self._condition: self._raise_if_remote_stop_requested_locked() @@ -690,6 +872,8 @@ def _raise_if_remote_stop_requested(self) -> None: def _raise_if_remote_stop_requested_locked(self) -> None: if self._emergency_stop: raise EmergencyStopError(f"remote {self.config.robot.emergency_stop_button!r} requested damping stop") + if self._passive_requested: + raise PassiveRequestedError("operator requested passive damping") if self._lie_down_requested: raise LieDownRequestedError(f"remote {self.config.robot.lie_down_button!r} requested lie-down shutdown") @@ -698,4 +882,25 @@ def _require_open(self) -> None: raise RuntimeError("Unitree backend is not open") -__all__ = ["UnitreeGo2Robot", "UnitreeSdkBindings"] +class UnitreeG1_29DOFRobot(UnitreeGo2Robot): + """Map canonical G1 29-DOF state and commands to HG DDS messages.""" + + robot_config_type = UnitreeG1_29DOFRobotCfg + disabled_position = 0.0 + disabled_velocity = 0.0 + + def _load_sdk_bindings(self) -> UnitreeSdkBindings: + return _load_g1_29dof_sdk_bindings() + + def _disable_high_level_motion_services(self, sdk: UnitreeSdkBindings) -> None: + # The validated G1 deployment enters low-level mode outside this process. + # HG mode_pr/mode_machine are then verified from LowState before output. + del sdk + + def _publish_pre_enable_command(self) -> None: + # Match the validated G1 Passive state: mode 1, measured q, kp 0, + # kd 3, zero desired velocity and feed-forward torque. + self._publish_damping() + + +__all__ = ["UnitreeG1_29DOFRobot", "UnitreeGo2Robot", "UnitreeSdkBindings"] diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/native.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/native.py new file mode 100644 index 00000000..401a9684 --- /dev/null +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/native.py @@ -0,0 +1,35 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Optional C++ Unitree backend bridge. + +This module is intentionally small: the native extension owns DDS and the +1 kHz command writer, while this adapter preserves the Motrix RobotInterface +contract. The extension is imported lazily so simulation and Python-SDK +diagnostic paths do not require a C++ SDK installation. +""" + +from __future__ import annotations + +from importlib import import_module +from typing import Any, Protocol + + +class NativeBackend(Protocol): + def open(self) -> None: ... + + def close(self) -> None: ... + + +def load_native_module() -> Any: + """Load the separately built C++ extension only when hardware is selected.""" + try: + return import_module("motrix_unitree_native") + except ImportError as error: + raise RuntimeError( + "The native Unitree backend requires the motrix_unitree_native extension. " + "Build it against Unitree SDK2 or select transport=python_sdk for diagnostics." + ) from error + + +__all__ = ["NativeBackend", "load_native_module"] diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/native_interface.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/native_interface.py new file mode 100644 index 00000000..e496fe3d --- /dev/null +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/native_interface.py @@ -0,0 +1,449 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""G1 hardware adapter backed by the native Unitree SDK2 extension. + +The extension owns DDS callbacks and the 1 kHz LowCmd writer. This module +only translates the native state snapshot into the existing Motrix contracts +at policy-loop frequency. +""" + +from __future__ import annotations + +import math +import time +from collections.abc import Callable +from dataclasses import dataclass +from typing import Any + +import numpy as np + +from motrix_deploy.contracts import ( + HealthStatus, + JointControlMode, + JointServoCommand, + JointTorqueCommand, + RobotCapabilities, + RobotCommand, + RobotSpec, + RobotState, +) +from motrix_deploy.errors import ( + EmergencyStopError, + LieDownRequestedError, + PassiveRequestedError, + ValidationError, +) +from motrix_deploy.operator import OperatorIntentEvaluator +from motrix_deploy.robot.interface import RobotInterface +from motrix_deploy_unitree.config import UnitreeBackendConfig, UnitreeG1_29DOFRobotCfg +from motrix_deploy_unitree.native import load_native_module +from motrix_deploy_unitree.remote import UnitreeRemoteGamePadDevice, UnitreeRemoteState, decode_wireless_remote +from motrix_env_core.input import GamePadDevice + +# Latched operator intents are only meaningful for the next few frames. +_LATCHED_INTENT_TTL_NS = 5_000_000_000 + + +@dataclass(frozen=True) +class NativeBackendHealth: + healthy: bool + reason: str + last_successful_communication_ns: int + + +def _snapshot_field(snapshot: object, name: str) -> object: + if isinstance(snapshot, dict): + return snapshot[name] + return getattr(snapshot, name) + + +class UnitreeG1NativeRobot(RobotInterface): + """Map the native C++ G1 transport to the Motrix RobotInterface contract.""" + + robot_config_type = UnitreeG1_29DOFRobotCfg + + def __init__( + self, + config: UnitreeBackendConfig, + control_period_s: float, + state_timeout_s: float, + *, + spec: RobotSpec, + native_backend: Any | None = None, + native_factory: Callable[..., Any] | None = None, + sleep: Callable[[float], None] = time.sleep, + ) -> None: + if not isinstance(config.robot, self.robot_config_type): + raise TypeError(f"{type(self).__name__} requires {self.robot_config_type.__name__}") + if spec.joint_count != 29: + raise ValidationError("robot.joint_count", 29, spec.joint_count) + for path, value in (("control.period_s", control_period_s), ("control.state_timeout_s", state_timeout_s)): + if not isinstance(value, (int, float)) or isinstance(value, bool) or not np.isfinite(value) or value <= 0: + raise ValidationError(path, "a positive finite number", value) + + self.config = config + self._spec = spec + self._control_period_s = float(control_period_s) + self._state_timeout_s = float(state_timeout_s) + self._native_backend = native_backend + self._native_factory = native_factory + self._sleep = sleep + self._motor_indices = np.empty(0, dtype=np.int64) + self._kp_override: np.ndarray | None = None + self._kd_override: np.ndarray | None = None + self._native: Any | None = None + self._latest_state: RobotState | None = None + self._last_read_sequence = -1 + self._remote = UnitreeRemoteState() + self._gamepad = UnitreeRemoteGamePadDevice(lambda: self.remote) + self._intent_evaluator = None if config.operator is None else OperatorIntentEvaluator(config.operator.intents) + # intent -> receive_time_ns; consumed by _wait_for_intent in one_shot mode + # and pruned by age, so state_machine mode cannot accumulate forever. + self._latched_intents: dict[str, int] = {} + self._emergency_stop = False + self._passive_requested = False + self._lie_down_requested = False + self._opened = False + self._enabled = False + self._stopped = False + self._closed = False + self._capabilities: RobotCapabilities | None = None + self._health_reason = "backend is not open" + + @property + def spec(self) -> RobotSpec: + return self._spec + + @property + def capabilities(self) -> RobotCapabilities: + if self._capabilities is None: + raise RuntimeError("Unitree capabilities are available after open()") + return self._capabilities + + @property + def remote(self) -> UnitreeRemoteState: + return self._remote + + def get_gamepad_device(self) -> GamePadDevice: + return self._gamepad + + def _make_native_backend(self) -> Any: + if self._native_backend is not None: + return self._native_backend + factory = self._native_factory + if factory is None: + factory = load_native_module().G1Backend + robot = self.config.robot + period = robot.command_publish_period_s or 0.001 + return factory( + network_interface=self.config.network_interface, + domain_id=self.config.domain_id, + lowcmd_topic=self.config.lowcmd_topic, + lowstate_topic=self.config.lowstate_topic, + motor_indices=self._motor_indices.tolist(), + mode_pr=-1 if robot.mode_pr is None else robot.mode_pr, + expected_mode_machine=-1 if robot.expected_mode_machine is None else robot.expected_mode_machine, + motor_mode=1 if robot.motor_mode is None else robot.motor_mode, + damping_kd=robot.damping_kd, + command_period_s=period, + command_timeout_s=robot.command_timeout_s, + state_timeout_s=self._state_timeout_s, + validate_crc=self.config.validate_crc, + subscriber_queue_depth=self.config.subscriber_queue_depth, + ) + + def open(self) -> None: + if self._opened and not self._closed: + raise RuntimeError("native Unitree backend is already open") + robot = self.config.robot + self._motor_indices = self.config.robot.motor_indices(self._spec.joint_names) + self._kp_override = robot.gain_override("kp", self._spec.joint_names) + self._kd_override = robot.gain_override("kd", self._spec.joint_names) + # A native backend starts its sequence at zero after a close/reopen. + # Reset the adapter-side cursor as well so the first sample cannot be + # rejected as older than the previous session. + self._last_read_sequence = -1 + self._latest_state = None + self._remote = UnitreeRemoteState() + self._latched_intents.clear() + self._emergency_stop = False + self._passive_requested = False + self._lie_down_requested = False + self._native = self._make_native_backend() + try: + self._native.open(self.config.connect_timeout_s) + except Exception: + try: + self._native.close() + finally: + self._closed = True + raise + self._opened = True + self._closed = False + self._enabled = False + self._stopped = False + self._health_reason = "" + self._capabilities = RobotCapabilities( + control_modes=(JointControlMode.SERVO, JointControlMode.TORQUE), + state_fields=frozenset( + { + "joint_position", + "joint_velocity", + "base_orientation_xyzw", + "base_angular_velocity", + "base_linear_acceleration", + } + ), + supports_rendering=False, + max_command_rate_hz=1.0 / self._control_period_s, + requires_enable=True, + stop_semantics="damping", + ) + + def read_state(self, timeout_s: float) -> RobotState: + self._require_open() + if timeout_s <= 0: + raise TimeoutError(f"Unitree state timeout must be positive, got {timeout_s}") + assert self._native is not None + try: + snapshot = self._native.read_state(timeout_s, self._last_read_sequence) + except TimeoutError: + raise + except Exception as error: + message = str(error) + if "timeout" in message.lower() or "no fresh" in message.lower(): + raise TimeoutError(message) from error + raise RuntimeError(message) from error + + sequence = int(_snapshot_field(snapshot, "sequence")) + self._last_read_sequence = sequence + state = self._state_from_snapshot(snapshot) + self._latest_state = state + self._update_remote(snapshot, state.receive_time_ns) + self._raise_if_remote_stop_requested() + return state + + def _state_from_snapshot(self, snapshot: object) -> RobotState: + def vector(name: str, shape: tuple[int, ...]) -> np.ndarray: + value = np.asarray(_snapshot_field(snapshot, name), dtype=np.float32) + if value.shape != shape or not np.all(np.isfinite(value)): + raise ValidationError(f"state.{name}", f"finite float32 values with shape {shape}", value) + return value + + quaternion = vector("quaternion_xyzw", (4,)) + norm = float(np.linalg.norm(quaternion)) + if norm <= 1.0e-8: + raise ValidationError("state.base_orientation_xyzw", "a non-zero quaternion", norm) + quaternion = (quaternion / np.float32(norm)).astype(np.float32, copy=False) + receive_time_ns = int(_snapshot_field(snapshot, "receive_time_ns")) + return RobotState( + sample_time_ns=receive_time_ns, + receive_time_ns=receive_time_ns, + joint_position=vector("joint_position", (29,)), + joint_velocity=vector("joint_velocity", (29,)), + base_orientation_xyzw=quaternion, + base_angular_velocity=vector("gyro", (3,)), + base_linear_acceleration=vector("accelerometer", (3,)), + ) + + def _update_remote(self, snapshot: object, receive_time_ns: int) -> None: + payload = _snapshot_field(snapshot, "wireless_remote") + self._remote = decode_wireless_remote(payload) + if self._intent_evaluator is not None: + intents = self._intent_evaluator.update(self._remote.pressed_buttons(), receive_time_ns) + for intent in intents: + self._latched_intents[intent] = receive_time_ns + cutoff_ns = receive_time_ns - _LATCHED_INTENT_TTL_NS + for intent, stamp in tuple(self._latched_intents.items()): + if stamp < cutoff_ns: + del self._latched_intents[intent] + if "emergency_stop" in intents or "request_emergency" in intents: + self._emergency_stop = True + if "enter_passive" in intents and self.config.robot.startup_mode == "one_shot": + self._passive_requested = True + if self._remote.pressed("select") or ( + self._intent_evaluator is None and self._remote.pressed(self.config.robot.emergency_stop_button) + ): + self._emergency_stop = True + if self.config.robot.allow_lie_down and self._remote.pressed(self.config.robot.lie_down_button): + self._lie_down_requested = True + + def enable(self) -> None: + self._require_open() + if self._enabled: + return + assert self._native is not None + if self.config.robot.startup_mode == "state_machine": + self._native.set_enabled(True) + self._enabled = True + return + if self._kp_override is None or self._kd_override is None: + raise ValidationError("backend.enable", "explicit backend kp and kd safety gains", "missing") + if self.config.robot.wait_for_remote_buttons: + if self._intent_evaluator is None: + self._wait_for_button(self.config.robot.start_button, self._publish_zero_torque) + else: + self._wait_for_intent("request_stand", self._publish_zero_torque) + state = self._latest_state if self._latest_state is not None else self.read_state(self._state_timeout_s) + zeros = np.zeros(29, dtype=np.float32) + self._native.set_enabled(True) + self._enabled = True + steps = max(1, math.ceil(self.config.robot.default_transition_duration_s / self._control_period_s)) + for step in range(1, steps + 1): + self._raise_if_remote_stop_requested() + alpha = np.float32(step / steps) + position = ( + (np.float32(1.0) - alpha) * state.joint_position + alpha * self._spec.default_joint_position + ).astype(np.float32) + self._write_servo(position, zeros, zeros, self._kp_override, self._kd_override) + self._wait_control_period() + if self.config.robot.wait_for_remote_buttons: + if self._intent_evaluator is None: + self._wait_for_button( + self.config.robot.enable_button, + lambda: self._write_servo( + self._spec.default_joint_position, zeros, zeros, self._kp_override, self._kd_override + ), + ) + else: + self._wait_for_intent( + "enable_policy", + lambda: self._write_servo( + self._spec.default_joint_position, zeros, zeros, self._kp_override, self._kd_override + ), + ) + self._raise_if_remote_stop_requested() + + def write_command(self, command: RobotCommand) -> None: + self._require_open() + if not self._enabled: + raise RuntimeError("Unitree command output is not enabled") + self._raise_if_remote_stop_requested() + assert self._native is not None + self._validate_command(command) + if isinstance(command, JointTorqueCommand): + self._native.write_torque(command.torque) + elif isinstance(command, JointServoCommand): + self._write_servo( + command.joint_position, + command.joint_velocity, + command.feedforward_torque, + command.kp, + command.kd, + ) + else: + raise ValidationError("command.type", "JointServoCommand or JointTorqueCommand", type(command).__name__) + + def _write_servo( + self, + position: np.ndarray, + velocity: np.ndarray, + torque: np.ndarray, + kp: np.ndarray, + kd: np.ndarray, + ) -> None: + assert self._native is not None + self._native.write_command(position, velocity, torque, kp, kd) + + def _validate_command(self, command: RobotCommand) -> None: + """Enforce the same canonical limits as the Python DDS backend.""" + shape = (self._spec.joint_count,) + if isinstance(command, JointTorqueCommand): + if command.torque.shape != shape or command.torque.dtype != np.float32: + raise ValidationError("command.torque", f"finite float32 shape {shape}", command.torque) + if not np.all(np.isfinite(command.torque)) or np.any(np.abs(command.torque) > self._spec.torque_limit): + raise ValidationError("command.torque", "inside RobotSpec torque limits", command.torque.tolist()) + return + if not isinstance(command, JointServoCommand): + raise ValidationError("command.type", "JointServoCommand or JointTorqueCommand", type(command).__name__) + for name in ("joint_position", "joint_velocity", "feedforward_torque", "kp", "kd"): + value = getattr(command, name) + if value.shape != shape or value.dtype != np.float32 or not np.all(np.isfinite(value)): + raise ValidationError(f"command.{name}", f"finite float32 shape {shape}", value) + if np.any(command.kp < 0) or np.any(command.kd < 0): + raise ValidationError("command.gains", "non-negative values", "negative gain") + if np.any(command.joint_position < self._spec.position_lower) or np.any( + command.joint_position > self._spec.position_upper + ): + raise ValidationError( + "command.joint_position", + "inside RobotSpec position range", + command.joint_position.tolist(), + ) + if np.any(np.abs(command.feedforward_torque) > self._spec.torque_limit): + raise ValidationError( + "command.feedforward_torque", + "inside RobotSpec torque limits", + command.feedforward_torque.tolist(), + ) + + def _publish_zero_torque(self) -> None: + assert self._native is not None + self._native.write_torque(np.zeros(29, dtype=np.float32)) + + def health(self) -> HealthStatus: + if self._native is None: + return HealthStatus(False, self._health_reason, 0) + value = self._native.health() + if isinstance(value, dict): + return HealthStatus( + healthy=bool(value["healthy"]), + reason=str(value["reason"]), + last_successful_communication_ns=int(value["last_successful_communication_ns"]), + ) + return NativeBackendHealth(**value) + + def stop(self) -> None: + if self._stopped: + return + self._enabled = False + if self._native is not None: + self._native.stop() + self._stopped = True + + def close(self) -> None: + if self._closed: + return + try: + self.stop() + finally: + if self._native is not None: + self._native.close() + self._closed = True + + def _wait_for_button(self, button: str, publish: Callable[[], None]) -> None: + while True: + self.read_state(self._state_timeout_s) + if self.remote.pressed(button): + return + self._raise_if_remote_stop_requested() + publish() + + def _wait_for_intent(self, intent: str, publish: Callable[[], None]) -> None: + while True: + self.read_state(self._state_timeout_s) + if intent in self._latched_intents: + del self._latched_intents[intent] + return + self._raise_if_remote_stop_requested() + publish() + + def _wait_control_period(self) -> None: + self._sleep(self._control_period_s) + self.read_state(self._state_timeout_s) + + def _raise_if_remote_stop_requested(self) -> None: + if self._emergency_stop: + raise EmergencyStopError(f"remote {self.config.robot.emergency_stop_button!r} requested damping stop") + if self._passive_requested: + raise PassiveRequestedError("operator requested passive damping") + if self._lie_down_requested: + raise LieDownRequestedError(f"remote {self.config.robot.lie_down_button!r} requested lie-down shutdown") + + def _require_open(self) -> None: + if not self._opened or self._closed: + raise RuntimeError("native Unitree backend is not open") + + +__all__ = ["NativeBackendHealth", "UnitreeG1NativeRobot"] diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/plugin.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/plugin.py index 6472a657..e9f5ded7 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/plugin.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/plugin.py @@ -5,8 +5,9 @@ from motrix_deploy.runtime.factory import RuntimeCreateContext from motrix_deploy.runtime.hardware import HardwareRuntime -from motrix_deploy_unitree.config import UnitreeBackendConfig, UnitreeGo2RobotCfg -from motrix_deploy_unitree.interface import UnitreeGo2Robot +from motrix_deploy_unitree.config import UnitreeBackendConfig, UnitreeG1_29DOFRobotCfg, UnitreeGo2RobotCfg +from motrix_deploy_unitree.interface import UnitreeG1_29DOFRobot, UnitreeGo2Robot +from motrix_deploy_unitree.native_interface import UnitreeG1NativeRobot def create_runtime( @@ -17,9 +18,15 @@ def create_runtime( if not isinstance(config, UnitreeBackendConfig): raise TypeError(f"Unitree runtime requires UnitreeBackendConfig, got {type(config).__name__}") - if not isinstance(config.robot, UnitreeGo2RobotCfg): + if isinstance(config.robot, UnitreeG1_29DOFRobotCfg): + robot_type = UnitreeG1NativeRobot if config.transport == "native_cpp" else UnitreeG1_29DOFRobot + elif isinstance(config.robot, UnitreeGo2RobotCfg): + if config.transport == "native_cpp": + raise ValueError("transport=native_cpp currently supports only the G1 29-DOF backend") + robot_type = UnitreeGo2Robot + else: raise TypeError(f"Unsupported Unitree robot config: {type(config.robot).__name__}") - robot = UnitreeGo2Robot( + robot = robot_type( config, control_period_s=context.control.period_s, state_timeout_s=context.control.state_timeout_s, diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/read_lowstate.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/read_lowstate.py index 6c0954af..3b1c844f 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/read_lowstate.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/read_lowstate.py @@ -1,18 +1,18 @@ # Copyright Motphys Technology Co., Ltd. 2025, 2026 # SPDX-License-Identifier: Apache-2.0 -"""Read Unitree Go2 LowState without creating a LowCmd publisher.""" +"""Read Unitree LowState without creating a LowCmd publisher.""" import sys import time from dataclasses import dataclass from typing import Any -from motrix_deploy_unitree.config import GO2_JOINT_NAME_TO_MOTOR_INDEX +from motrix_deploy_unitree.config import G1_29DOF_JOINT_NAME_TO_MOTOR_ID, GO2_JOINT_NAME_TO_MOTOR_INDEX from motrix_deploy_unitree.remote import BUTTON_NAMES, decode_wireless_remote -JOINT_NAMES = tuple(GO2_JOINT_NAME_TO_MOTOR_INDEX) -MOTOR_INDICES = tuple(GO2_JOINT_NAME_TO_MOTOR_INDEX.values()) +GO2_JOINT_NAMES = tuple(GO2_JOINT_NAME_TO_MOTOR_INDEX) +GO2_MOTOR_INDICES = tuple(GO2_JOINT_NAME_TO_MOTOR_INDEX.values()) @dataclass @@ -26,10 +26,28 @@ class ReadStatistics: class LowStatePrinter: - """Validate, throttle, and print canonical Go2 state samples.""" - - def __init__(self, crc: Any, *, print_interval_s: float, validate_crc: bool) -> None: + """Validate, throttle, and print canonical Unitree state samples.""" + + def __init__( + self, + crc: Any, + *, + robot: str, + joint_names: tuple[str, ...], + motor_indices: tuple[int, ...], + motor_slot_count: int, + expected_mode_pr: int | None, + expected_mode_machine: int | None, + print_interval_s: float, + validate_crc: bool, + ) -> None: self.crc = crc + self.robot = robot + self.joint_names = joint_names + self.motor_indices = motor_indices + self.motor_slot_count = motor_slot_count + self.expected_mode_pr = expected_mode_pr + self.expected_mode_machine = expected_mode_machine self.print_interval_ns = round(print_interval_s * 1e9) self.validate_crc = validate_crc self.statistics = ReadStatistics() @@ -41,10 +59,16 @@ def __call__(self, message: Any) -> None: if self.validate_crc and self.crc.Crc(message) != message.crc: self.statistics.crc_errors += 1 return - if len(message.motor_state) < 12: - raise ValueError(f"expected at least 12 motor states, got {len(message.motor_state)}") - position = [float(message.motor_state[index].q) for index in MOTOR_INDICES] - velocity = [float(message.motor_state[index].dq) for index in MOTOR_INDICES] + if len(message.motor_state) < self.motor_slot_count: + raise ValueError( + f"expected at least {self.motor_slot_count} motor states, got {len(message.motor_state)}" + ) + if self.expected_mode_pr is not None and message.mode_pr != self.expected_mode_pr: + raise ValueError(f"mode_pr={message.mode_pr}, expected {self.expected_mode_pr}") + if self.expected_mode_machine is not None and message.mode_machine != self.expected_mode_machine: + raise ValueError(f"mode_machine={message.mode_machine}, expected {self.expected_mode_machine}") + position = [float(message.motor_state[index].q) for index in self.motor_indices] + velocity = [float(message.motor_state[index].dq) for index in self.motor_indices] quaternion_wxyz = [float(value) for value in message.imu_state.quaternion] gyroscope = [float(value) for value in message.imu_state.gyroscope] accelerometer = [float(value) for value in message.imu_state.accelerometer] @@ -72,12 +96,14 @@ def __call__(self, message: Any) -> None: except Exception as error: remote_text = f"unavailable ({error})" - print("\n--- Unitree Go2 LowState ---") + print(f"\n--- Unitree {self.robot} LowState ---") print( f"frames received={self.statistics.received} valid={self.statistics.valid} " f"crc_errors={self.statistics.crc_errors} decode_errors={self.statistics.decode_errors}" ) - for name, joint_position, joint_velocity in zip(JOINT_NAMES, position, velocity, strict=True): + if self.expected_mode_pr is not None: + print(f"mode_pr={message.mode_pr} mode_machine={message.mode_machine}") + for name, joint_position, joint_velocity in zip(self.joint_names, position, velocity, strict=True): print(f"{name:16s} q={joint_position: 9.5f} rad dq={joint_velocity: 9.5f} rad/s") print("imu quaternion wxyz:", quaternion_wxyz) print("imu quaternion xyzw:", quaternion_wxyz[1:] + quaternion_wxyz[:1]) @@ -86,11 +112,15 @@ def __call__(self, message: Any) -> None: print("remote:", remote_text, flush=True) -def _load_sdk() -> tuple[Any, Any, Any, Any]: +def _load_sdk(robot: str) -> tuple[Any, Any, Any, Any]: try: from unitree_sdk2py.core.channel import ChannelFactoryInitialize, ChannelSubscriber - from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ from unitree_sdk2py.utils.crc import CRC + + if robot == "g1-29dof": + from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_ + else: + from unitree_sdk2py.idl.unitree_go.msg.dds_ import LowState_ except ImportError as error: raise RuntimeError( "LowState reading requires the official unitree_sdk2py package; install " @@ -108,16 +138,39 @@ def run_read_lowstate( duration_s: float, print_interval_s: float, validate_crc: bool, + robot: str = "go2", ) -> int: """Run a bounded read-only LowState DDS diagnostic.""" try: - channel_factory_initialize, channel_subscriber, low_state_type, crc_type = _load_sdk() + channel_factory_initialize, channel_subscriber, low_state_type, crc_type = _load_sdk(robot) except RuntimeError as error: print(error, file=sys.stderr) return 2 channel_factory_initialize(domain_id, network_interface) - printer = LowStatePrinter(crc_type(), print_interval_s=print_interval_s, validate_crc=validate_crc) + if robot == "g1-29dof": + joint_names = tuple(G1_29DOF_JOINT_NAME_TO_MOTOR_ID) + motor_indices = tuple(G1_29DOF_JOINT_NAME_TO_MOTOR_ID.values()) + motor_slot_count = 35 + expected_mode_pr = 0 + expected_mode_machine = 5 + else: + joint_names = GO2_JOINT_NAMES + motor_indices = GO2_MOTOR_INDICES + motor_slot_count = 12 + expected_mode_pr = None + expected_mode_machine = None + printer = LowStatePrinter( + crc_type(), + robot=robot, + joint_names=joint_names, + motor_indices=motor_indices, + motor_slot_count=motor_slot_count, + expected_mode_pr=expected_mode_pr, + expected_mode_machine=expected_mode_machine, + print_interval_s=print_interval_s, + validate_crc=validate_crc, + ) subscriber = channel_subscriber(topic, low_state_type) subscriber.Init(printer, queue_depth) duration = "unlimited time" if duration_s == 0 else f"{duration_s:g} seconds" diff --git a/motrix_deploy_unitree/src/motrix_deploy_unitree/remote.py b/motrix_deploy_unitree/src/motrix_deploy_unitree/remote.py index 2ca76e0e..8e2410eb 100644 --- a/motrix_deploy_unitree/src/motrix_deploy_unitree/remote.py +++ b/motrix_deploy_unitree/src/motrix_deploy_unitree/remote.py @@ -30,6 +30,13 @@ "down", "left", ) +BUTTON_ALIASES = {"RB": "R1", "LB": "L1", "RT": "R2", "LT": "L2", "back": "select"} +ACCEPTED_BUTTON_NAMES = BUTTON_NAMES + tuple(BUTTON_ALIASES) + + +def canonical_button_name(name: str) -> str: + """Resolve SDK-wrapper aliases without changing the stable wire bit names.""" + return BUTTON_ALIASES.get(name, name) @dataclass(frozen=True) @@ -43,12 +50,19 @@ class UnitreeRemoteState: ry: float = 0.0 def pressed(self, name: str) -> bool: + name = canonical_button_name(name) try: index = BUTTON_NAMES.index(name) except ValueError as error: raise ValidationError("remote.button", f"one of {list(BUTTON_NAMES)}", name) from error return bool(self.keys & (1 << index)) + def pressed_buttons(self) -> frozenset[str]: + """Return active canonical names plus their accepted SDK aliases.""" + active = {name for name in BUTTON_NAMES if self.pressed(name)} + active.update(alias for alias, canonical in BUTTON_ALIASES.items() if canonical in active) + return frozenset(active) + class UnitreeRemoteGamePadDevice(GamePadDevice): """Expose the latest Unitree remote sample through the common gamepad API.""" @@ -98,4 +112,12 @@ def decode_wireless_remote(data: object) -> UnitreeRemoteState: return UnitreeRemoteState(keys=keys, lx=float(lx), ly=float(ly), rx=float(rx), ry=float(ry)) -__all__ = ["BUTTON_NAMES", "UnitreeRemoteGamePadDevice", "UnitreeRemoteState", "decode_wireless_remote"] +__all__ = [ + "ACCEPTED_BUTTON_NAMES", + "BUTTON_ALIASES", + "BUTTON_NAMES", + "UnitreeRemoteGamePadDevice", + "UnitreeRemoteState", + "canonical_button_name", + "decode_wireless_remote", +] diff --git a/motrix_deploy_unitree/tests/test_g1_29dof_backend.py b/motrix_deploy_unitree/tests/test_g1_29dof_backend.py new file mode 100644 index 00000000..6c740691 --- /dev/null +++ b/motrix_deploy_unitree/tests/test_g1_29dof_backend.py @@ -0,0 +1,319 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""G1 29-DOF HG message, mode, and named-joint mapping contracts.""" + +import copy +import time +from types import SimpleNamespace + +import numpy as np +import pytest + +from motrix_deploy.contracts import JointServoCommand, RobotSpec +from motrix_deploy.errors import EmergencyStopError, PassiveRequestedError +from motrix_deploy.operator import ButtonGestureCfg, OperatorProfileCfg +from motrix_deploy.runtime.factory import RuntimeCreateContext +from motrix_deploy.runtime.hardware import HardwareRuntime +from motrix_deploy_unitree.config import UnitreeBackendConfig, UnitreeG1_29DOFRobotCfg +from motrix_deploy_unitree.hardware import G1_29DOF_JOINT_NAMES, UnitreeG1_29DOFHardwareCfg +from motrix_deploy_unitree.interface import UnitreeG1_29DOFRobot, UnitreeSdkBindings +from motrix_deploy_unitree.plugin import create_runtime + + +class _Motor: + def __init__(self, index: int = 0) -> None: + self.mode = 0 + self.q = float(index) + self.dq = float(-index) + self.kp = 0.0 + self.kd = 0.0 + self.tau = 0.0 + + +class _LowCmd: + def __init__(self) -> None: + self.mode_pr = 0 + self.mode_machine = 0 + self.motor_cmd = [_Motor() for _ in range(35)] + self.crc = 0 + + +class _LowState: + def __init__(self, *, mode_pr: int = 0, mode_machine: int = 5) -> None: + self.mode_pr = mode_pr + self.mode_machine = mode_machine + self.motor_state = [_Motor(index) for index in range(35)] + self.imu_state = SimpleNamespace( + quaternion=[1.0, 0.0, 0.0, 0.0], + gyroscope=[0.1, 0.2, 0.3], + accelerometer=[0.0, 0.0, 9.81], + ) + self.wireless_remote = bytes(40) + self.crc = 123 + + +class _CRC: + def Crc(self, message: object) -> int: + del message + return 123 + + +class _Publisher: + def __init__(self, bus: "_Bus", topic: str, message_type: type) -> None: + self.bus = bus + self.topic = topic + self.message_type = message_type + + def Init(self) -> None: + pass + + def Write(self, message: _LowCmd) -> None: + self.bus.commands.append(copy.deepcopy(message)) + if self.bus.callback is not None: + self.bus.callback(copy.deepcopy(self.bus.state)) + + def Close(self) -> None: + pass + + +class _Subscriber: + def __init__(self, bus: "_Bus", topic: str, message_type: type) -> None: + self.bus = bus + self.topic = topic + self.message_type = message_type + + def Init(self, callback, queue_depth: int) -> None: + self.bus.callback = callback + self.bus.queue_depth = queue_depth + callback(copy.deepcopy(self.bus.state)) + + def Close(self) -> None: + pass + + +class _Bus: + def __init__(self, *, mode_pr: int = 0, mode_machine: int = 5) -> None: + self.state = _LowState(mode_pr=mode_pr, mode_machine=mode_machine) + self.commands: list[_LowCmd] = [] + self.callback = None + self.queue_depth = 0 + + def sdk(self) -> UnitreeSdkBindings: + return UnitreeSdkBindings( + channel_factory_initialize=lambda domain, interface: None, + channel_publisher=lambda topic, message_type: _Publisher(self, topic, message_type), + channel_subscriber=lambda topic, message_type: _Subscriber(self, topic, message_type), + low_cmd_message_type=_LowCmd, + low_state_message_type=_LowState, + make_low_cmd=_LowCmd, + crc_type=_CRC, + ) + + +def _spec(joint_names: tuple[str, ...] = G1_29DOF_JOINT_NAMES) -> RobotSpec: + count = len(joint_names) + return RobotSpec( + base_link_name="torso_link", + joint_names=joint_names, + default_joint_position=np.zeros(count, dtype=np.float32), + position_lower=np.full(count, -10.0, dtype=np.float32), + position_upper=np.full(count, 10.0, dtype=np.float32), + torque_limit=np.full(count, 100.0, dtype=np.float32), + ) + + +def _config(**overrides) -> UnitreeBackendConfig: + robot_values = { + "wait_for_remote_buttons": False, + "default_transition_duration_s": 0.02, + "damping_duration_s": 0.0, + "kp": 20.0, + "kd": 1.0, + "command_publish_period_s": None, + } + robot_values.update(overrides) + return UnitreeBackendConfig( + network_interface="eth0", + robot=UnitreeG1_29DOFRobotCfg(**robot_values), + ) + + +def _remote_payload(*buttons: str) -> bytes: + from motrix_deploy_unitree.remote import BUTTON_NAMES, canonical_button_name + + keys = sum(1 << BUTTON_NAMES.index(canonical_button_name(button)) for button in buttons) + payload = bytearray(40) + payload[2:4] = keys.to_bytes(2, "little") + return bytes(payload) + + +def _backend(config: UnitreeBackendConfig, bus: _Bus, *, spec: RobotSpec | None = None) -> UnitreeG1_29DOFRobot: + return UnitreeG1_29DOFRobot( + config, + control_period_s=0.02, + state_timeout_s=0.1, + spec=_spec() if spec is None else spec, + sdk=bus.sdk(), + sleep=lambda _: None, + ) + + +def test_g1_hardware_uses_explicit_joint_name_to_motor_id_mapping() -> None: + hardware = UnitreeG1_29DOFHardwareCfg() + hardware.validate() + assert tuple(hardware.key_pose.joint_names) == G1_29DOF_JOINT_NAMES + assert {name: binding.index for name, binding in hardware.actuation.motors.items()} == { + name: motor_id for motor_id, name in enumerate(G1_29DOF_JOINT_NAMES) + } + assert all(binding.enabled_mode == 1 for binding in hardware.actuation.motors.values()) + + +def test_g1_state_and_command_follow_artifact_joint_names_not_policy_indices() -> None: + joint_names = G1_29DOF_JOINT_NAMES[7:] + G1_29DOF_JOINT_NAMES[:7] + bus = _Bus() + backend = _backend(_config(), bus, spec=_spec(joint_names)) + backend.open() + state = backend.read_state(0.1) + expected_motor_ids = [G1_29DOF_JOINT_NAMES.index(name) for name in joint_names] + assert state.joint_position.tolist() == pytest.approx(expected_motor_ids) + + backend.enable() + target = np.arange(29, dtype=np.float32) / 100.0 + zeros = np.zeros(29, dtype=np.float32) + policy_kp = np.full(29, 17.0, dtype=np.float32) + policy_kd = np.full(29, 0.7, dtype=np.float32) + backend.write_command( + JointServoCommand( + joint_position=target, + joint_velocity=zeros, + feedforward_torque=zeros, + kp=policy_kp, + kd=policy_kd, + ) + ) + command = bus.commands[-1] + assert command.mode_pr == 0 + assert command.mode_machine == 5 + for policy_index, motor_id in enumerate(expected_motor_ids): + assert command.motor_cmd[motor_id].q == pytest.approx(target[policy_index]) + assert command.motor_cmd[motor_id].kp == pytest.approx(policy_kp[policy_index]) + assert command.motor_cmd[motor_id].kd == pytest.approx(policy_kd[policy_index]) + for motor in command.motor_cmd[29:]: + assert (motor.mode, motor.q, motor.dq, motor.kp, motor.kd, motor.tau) == (0, 0.0, 0.0, 0.0, 0.0, 0.0) + backend.close() + + +def test_named_startup_gains_follow_artifact_joint_order() -> None: + joint_names = G1_29DOF_JOINT_NAMES[7:] + G1_29DOF_JOINT_NAMES[:7] + kp = {name: float(index + 1) for index, name in enumerate(G1_29DOF_JOINT_NAMES)} + kd = {name: float(index + 1) / 10.0 for index, name in enumerate(G1_29DOF_JOINT_NAMES)} + bus = _Bus() + backend = _backend(_config(kp=kp, kd=kd), bus, spec=_spec(joint_names)) + backend.open() + backend.enable() + + command = bus.commands[-1] + for name in joint_names: + motor_id = G1_29DOF_JOINT_NAMES.index(name) + assert command.motor_cmd[motor_id].kp == pytest.approx(kp[name]) + assert command.motor_cmd[motor_id].kd == pytest.approx(kd[name]) + backend.close() + + +def test_state_machine_startup_enables_command_stream_without_legacy_pose_ramp() -> None: + bus = _Bus() + backend = _backend(_config(startup_mode="state_machine", kp=None, kd=None), bus) + backend.open() + backend.enable() + + zeros = np.zeros(29, dtype=np.float32) + backend.write_command(JointServoCommand(zeros, zeros, zeros, zeros, np.full(29, 3.0, dtype=np.float32))) + + assert len(bus.commands) == 1 + assert all(motor.kp == 0.0 and motor.kd == pytest.approx(3.0) for motor in bus.commands[-1].motor_cmd[:29]) + backend.close() + + +def test_operator_alias_gesture_requests_passive_damping() -> None: + profile = OperatorProfileCfg( + intents={ + "request_stand": ButtonGestureCfg(held=["LT"], pressed="up"), + "enable_policy": ButtonGestureCfg(held=["RB"], pressed="X"), + "enter_passive": ButtonGestureCfg(held=["LT"], pressed="B"), + "emergency_stop": ButtonGestureCfg(pressed="select"), + } + ) + config = _config() + config.operator = profile + config.__post_init__() + bus = _Bus() + backend = _backend(config, bus) + backend.open() + bus.state.wireless_remote = _remote_payload("LT") + bus.callback(copy.deepcopy(bus.state)) + bus.state.wireless_remote = _remote_payload("LT", "B") + bus.callback(copy.deepcopy(bus.state)) + + with pytest.raises(PassiveRequestedError): + backend.read_state(0.1) + backend.close() + + +def test_select_remains_an_unconditional_emergency_stop() -> None: + profile = OperatorProfileCfg( + intents={ + "request_stand": ButtonGestureCfg(held=["LT"], pressed="up"), + "enable_policy": ButtonGestureCfg(held=["RB"], pressed="X"), + "enter_passive": ButtonGestureCfg(held=["LT"], pressed="B"), + "emergency_stop": ButtonGestureCfg(pressed="A"), + } + ) + config = _config() + config.operator = profile + config.__post_init__() + bus = _Bus() + backend = _backend(config, bus) + backend.open() + bus.state.wireless_remote = _remote_payload("select") + bus.callback(copy.deepcopy(bus.state)) + + with pytest.raises(EmergencyStopError): + backend.read_state(0.1) + backend.close() + + +@pytest.mark.parametrize(("mode_pr", "mode_machine"), [(1, 5), (0, 4)]) +def test_g1_rejects_lowstate_mode_mismatch_before_enable(mode_pr: int, mode_machine: int) -> None: + backend = _backend(_config(), _Bus(mode_pr=mode_pr, mode_machine=mode_machine)) + with pytest.raises(RuntimeError, match="mode_pr|mode_machine"): + backend.open() + backend.close() + + +def test_g1_plugin_constructs_explicit_robot_type() -> None: + context = RuntimeCreateContext(robot=_spec(), control=SimpleNamespace(period_s=0.02, state_timeout_s=0.1)) + runtime = create_runtime(_config(), context) + assert isinstance(runtime, HardwareRuntime) + assert type(runtime.robot) is UnitreeG1_29DOFRobot + + +def test_g1_periodic_writer_repeats_latest_command() -> None: + bus = _Bus() + backend = _backend( + _config(command_publish_period_s=0.001, command_timeout_s=0.1), + bus, + ) + backend.open() + deadline = time.monotonic() + 0.05 + while len(bus.commands) < 3 and time.monotonic() < deadline: + time.sleep(0.001) + passive = bus.commands[0] + for motor_id in range(29): + assert passive.motor_cmd[motor_id].mode == 1 + assert passive.motor_cmd[motor_id].q == pytest.approx(bus.state.motor_state[motor_id].q) + assert passive.motor_cmd[motor_id].kp == 0.0 + assert passive.motor_cmd[motor_id].kd == 3.0 + backend.close() + assert len(bus.commands) >= 3 + assert all(command.crc == 123 for command in bus.commands) diff --git a/motrix_deploy_unitree/tests/test_native_backend.py b/motrix_deploy_unitree/tests/test_native_backend.py new file mode 100644 index 00000000..ec11a591 --- /dev/null +++ b/motrix_deploy_unitree/tests/test_native_backend.py @@ -0,0 +1,154 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Contract tests for the Python adapter around the native G1 transport.""" + +from __future__ import annotations + +import numpy as np +import pytest + +from motrix_deploy.artifact.schema import ControlSpec +from motrix_deploy.contracts import JointServoCommand, RobotSpec +from motrix_deploy.errors import ValidationError +from motrix_deploy.runtime.factory import RuntimeCreateContext +from motrix_deploy_unitree.config import UnitreeBackendConfig, UnitreeG1_29DOFRobotCfg +from motrix_deploy_unitree.hardware import G1_29DOF_JOINT_NAMES +from motrix_deploy_unitree.native_interface import UnitreeG1NativeRobot +from motrix_deploy_unitree.plugin import create_runtime + + +class _NativeFake: + def __init__(self) -> None: + self.sequence = 0 + self.enabled = False + self.commands: list[tuple[str, tuple[np.ndarray, ...]]] = [] + self.closed = False + + def open(self, timeout_s: float) -> None: + assert timeout_s > 0 + + def read_state(self, timeout_s: float, after_sequence: int) -> dict[str, object]: + assert timeout_s > 0 + self.sequence = max(self.sequence + 1, after_sequence + 1) + return { + "sequence": self.sequence, + "receive_time_ns": 1_000_000 + self.sequence, + "joint_position": np.arange(29, dtype=np.float32), + "joint_velocity": -np.arange(29, dtype=np.float32), + "quaternion_xyzw": np.asarray([0.0, 0.0, 0.0, 2.0], dtype=np.float32), + "gyro": np.asarray([0.1, 0.2, 0.3], dtype=np.float32), + "accelerometer": np.asarray([0.0, 0.0, 9.81], dtype=np.float32), + "wireless_remote": bytes(40), + } + + def write_command(self, *values: np.ndarray) -> None: + self.commands.append(("servo", tuple(np.asarray(value) for value in values))) + + def write_torque(self, torque: np.ndarray) -> None: + self.commands.append(("torque", (np.asarray(torque),))) + + def set_enabled(self, enabled: bool) -> None: + self.enabled = enabled + + def stop(self) -> None: + self.enabled = False + + def close(self) -> None: + self.closed = True + + def health(self) -> dict[str, object]: + return { + "healthy": not self.closed, + "reason": "" if not self.closed else "backend is closed", + "last_successful_communication_ns": 1_000_001, + } + + +def _spec() -> RobotSpec: + return RobotSpec( + base_link_name="pelvis", + joint_names=G1_29DOF_JOINT_NAMES, + default_joint_position=np.zeros(29, dtype=np.float32), + position_lower=np.full(29, -10.0, dtype=np.float32), + position_upper=np.full(29, 10.0, dtype=np.float32), + torque_limit=np.full(29, 100.0, dtype=np.float32), + ) + + +def _config() -> UnitreeBackendConfig: + return UnitreeBackendConfig( + transport="native_cpp", + network_interface="eth0", + robot=UnitreeG1_29DOFRobotCfg( + wait_for_remote_buttons=False, + startup_mode="state_machine", + kp=20.0, + kd=1.0, + ), + ) + + +def test_native_adapter_normalizes_snapshot_and_forwards_command() -> None: + native = _NativeFake() + robot = UnitreeG1NativeRobot(_config(), 0.02, 0.1, spec=_spec(), native_backend=native, sleep=lambda _: None) + robot.open() + state = robot.read_state(0.1) + assert state.joint_position.shape == (29,) + np.testing.assert_array_equal(state.joint_position[:3], np.asarray([0.0, 1.0, 2.0], dtype=np.float32)) + np.testing.assert_allclose(state.base_orientation_xyzw, [0.0, 0.0, 0.0, 1.0]) + + robot.enable() + zeros = np.zeros(29, dtype=np.float32) + robot.write_command(JointServoCommand(zeros, zeros, zeros, np.ones(29, np.float32), np.ones(29, np.float32))) + assert native.enabled + assert native.commands[-1][0] == "servo" + robot.close() + assert native.closed + + +def test_native_factory_receives_configured_damping_gain() -> None: + """The damping kd must come from the runtime config, not a hardcoded value.""" + + recorded: dict[str, object] = {} + + class _Recorder(_NativeFake): + def __init__(self, **kwargs: object) -> None: + super().__init__() + recorded.update(kwargs) + + config = _config() + robot = UnitreeG1NativeRobot(config, 0.02, 0.1, spec=_spec(), native_factory=_Recorder, sleep=lambda _: None) + robot.open() + robot.close() + assert recorded["damping_kd"] == config.robot.damping_kd + assert recorded["command_period_s"] == config.robot.command_publish_period_s + assert recorded["command_timeout_s"] == config.robot.command_timeout_s + + +def test_native_transport_selects_g1_native_robot() -> None: + context = RuntimeCreateContext( + control=ControlSpec(period_s=0.02, state_timeout_s=0.1), + robot=_spec(), + ) + runtime = create_runtime(_config(), context) + assert isinstance(runtime.robot, UnitreeG1NativeRobot) + + +def test_native_adapter_enforces_robot_command_limits() -> None: + native = _NativeFake() + robot = UnitreeG1NativeRobot(_config(), 0.02, 0.1, spec=_spec(), native_backend=native, sleep=lambda _: None) + robot.open() + robot.enable() + zeros = np.zeros(29, dtype=np.float32) + with pytest.raises(ValidationError): + robot.write_command( + JointServoCommand( + joint_position=np.full(29, 11.0, dtype=np.float32), + joint_velocity=zeros, + feedforward_torque=zeros, + kp=np.ones(29, dtype=np.float32), + kd=np.ones(29, dtype=np.float32), + ) + ) + robot.close() diff --git a/motrix_deploy_unitree/tests/test_unitree_deploy_backend.py b/motrix_deploy_unitree/tests/test_unitree_deploy_backend.py index 6113a8e3..2be8f10c 100644 --- a/motrix_deploy_unitree/tests/test_unitree_deploy_backend.py +++ b/motrix_deploy_unitree/tests/test_unitree_deploy_backend.py @@ -167,7 +167,7 @@ def Write(self, message: _LowCmd) -> None: for motor_index, motor in enumerate(snapshot.motor_cmd): if motor.kp > 0: self.bus.state.motor_state[motor_index].q = motor.q - self.bus.state.motor_state[motor_index].dq = motor.qd + self.bus.state.motor_state[motor_index].dq = motor.dq self.bus.emit() def Close(self) -> None: @@ -382,7 +382,7 @@ def test_lowstate_and_lowcmd_are_mapped_by_canonical_joint_name() -> None: for canonical_index, name in enumerate(JOINT_NAMES): motor = published.motor_cmd[MOTOR_MAPPING[name]] assert motor.q == pytest.approx(command.joint_position[canonical_index]) - assert motor.qd == pytest.approx(0.0) + assert motor.dq == pytest.approx(0.0) assert motor.kp == pytest.approx(35.0) assert motor.kd == pytest.approx(0.5) assert published.crc == 123 @@ -430,7 +430,7 @@ def test_torque_enable_uses_servo_transition_then_maps_pure_torque_and_preserves backend.write_command(torque) for index, name in enumerate(JOINT_NAMES): motor = bus.commands[-1].motor_cmd[MOTOR_MAPPING[name]] - assert (motor.q, motor.qd, motor.kp, motor.kd) == (0.0, 0.0, 0.0, 0.0) + assert (motor.q, motor.dq, motor.kp, motor.kd) == (0.0, 0.0, 0.0, 0.0) assert motor.tau == pytest.approx(torque.torque[index]) assert motor.mode == transition.motor_cmd[MOTOR_MAPPING[name]].mode count = len(bus.commands) diff --git a/motrix_envs/src/motrix_envs/deploy/__init__.py b/motrix_envs/src/motrix_envs/deploy/__init__.py index f891b098..507eefca 100644 --- a/motrix_envs/src/motrix_envs/deploy/__init__.py +++ b/motrix_envs/src/motrix_envs/deploy/__init__.py @@ -7,9 +7,11 @@ def register() -> None: """Register the built-in environment-to-profile compiler mappings.""" from motrix_deploy.profile import register_profile_compiler + from motrix_envs.deploy.g1_walk import build_g1_walk_profile from motrix_envs.deploy.g1_wbt import build_g1_wbt_profile from motrix_envs.deploy.go2_walk import build_go2_walk_profile register_profile_compiler("go2-walk-flat", build_go2_walk_profile) register_profile_compiler("go2-walk-rough", build_go2_walk_profile) register_profile_compiler("g1-wbt-dance", build_g1_wbt_profile) + register_profile_compiler("g1-walk-flat", build_g1_walk_profile) diff --git a/motrix_envs/src/motrix_envs/deploy/g1_walk.py b/motrix_envs/src/motrix_envs/deploy/g1_walk.py new file mode 100644 index 00000000..48c6f540 --- /dev/null +++ b/motrix_envs/src/motrix_envs/deploy/g1_walk.py @@ -0,0 +1,176 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Compile the G1 flat-ground walking environment into its deploy contract.""" + +import math + +import numpy as np +from motrix_robots.unitree import UnitreeG129Dof + +from motrix_deploy.artifact import ControlSpec +from motrix_deploy.profile import DeploymentProfile +from motrix_deploy_tasks.tasks.g1_walk import G1WalkTaskSpec +from motrix_env_core import registry +from motrix_env_core.mdp.action import JointPositionActionTerm +from motrix_envs.deploy.robot import build_robot_model, build_robot_spec, read_position_servo_gains +from motrix_envs.locomotion.humanoid.cfg import HumanoidVelocityTrackingManagerEnvCfg + +_ACTION_TERM_NAME = "joint_position" +_COMMAND_TERM_NAME = "walk" + +# ``WalkCommand._lane_phase`` uses this threshold to pin both phase lanes +# while the planar velocity command is effectively zero. Keep it explicit in +# the exported contract rather than making the deployment task guess it. +_STANDING_COMMAND_THRESHOLD = 0.01 + +# Deployment uses one deterministic pair of antiphase clocks. Training adds a +# random common phase at reset, but that reset-only augmentation is not an +# operator/runtime input and therefore is not exported. +_GAIT_PHASE_OFFSETS_RAD = (0.0, math.pi) + + +def build_g1_walk_profile(env_name: str) -> DeploymentProfile: + """Compile the registered G1 walk task into canonical artifact data. + + The environment is constructed once so the compiler reads the exact + manager-generated action space, observation size, actuator order, and + per-joint action scales used by training. Values written to the artifact + are always reordered/validated against ``RobotSpec.joint_names``. + + ``G1WalkTaskSpec`` is deliberately implemented in the deployment package, + not here. Until that schema declares the fields assembled below this + function fails before emitting an incomplete artifact. + """ + + if env_name != "g1-walk-flat": + raise ValueError(f"G1 walk profile supports only 'g1-walk-flat', got {env_name!r}") + + env = registry.make(env_name, num_envs=1, mode="train") + cfg = env.cfg + if not isinstance(cfg, HumanoidVelocityTrackingManagerEnvCfg): + raise TypeError(f"Expected HumanoidVelocityTrackingManagerEnvCfg, got {type(cfg).__name__}") + + robot_cfg = cfg.scene.objs.robot + if not isinstance(robot_cfg, UnitreeG129Dof): + raise TypeError(f"Expected UnitreeG129Dof, got {type(robot_cfg).__name__}") + + model = build_robot_model(robot_cfg) + robot = build_robot_spec(robot_cfg, key_pose_name="default", model=model) + if robot.joint_count != 29: + raise ValueError(f"G1 29-DOF deployment requires 29 canonical joints, got {robot.joint_count}") + kp, kd = read_position_servo_gains(model, robot.joint_names) + + action_term = _single_joint_position_action_term(env) + action_joint_names = tuple(actuator.target_name for actuator in env.model.actuators) + if action_joint_names != robot.joint_names: + raise ValueError( + "G1 walk policy actuator targets must exactly match canonical robot joint order: " + f"policy={action_joint_names}, robot={robot.joint_names}" + ) + + action_shape = env.action_space.shape + observation_shape = env.policy_observation_space.shape + expected_action_shape = (robot.joint_count,) + if action_shape != expected_action_shape: + raise ValueError(f"G1 walk action space must have shape {expected_action_shape}, got {action_shape}") + if len(observation_shape) != 1 or observation_shape[0] <= 0: + raise ValueError(f"G1 walk policy observation space must be one non-empty vector, got {observation_shape}") + + action_state = action_term.state + _assert_vector("action default angles", action_state.default_angles, robot.joint_count) + _assert_vector("action scales", action_state.action_scales, robot.joint_count, non_negative=True) + _assert_vector("action lower bounds", env.action_space.low, robot.joint_count) + _assert_vector("action upper bounds", env.action_space.high, robot.joint_count) + if not np.allclose(action_state.default_angles, robot.default_joint_position, atol=1e-6, rtol=0.0): + raise ValueError("G1 walk action default angles do not match RobotSpec.default_joint_position") + if not np.allclose(action_state.joint_lower, robot.position_lower, atol=1e-6, rtol=0.0): + raise ValueError("G1 walk action lower joint limits do not match RobotSpec.position_lower") + if not np.allclose(action_state.joint_upper, robot.position_upper, atol=1e-6, rtol=0.0): + raise ValueError("G1 walk action upper joint limits do not match RobotSpec.position_upper") + + command_cfg = cfg.commands.walk + command_bounds = np.asarray(command_cfg.vel_limit, dtype=np.float32) + if command_bounds.shape != (2, 3) or not np.all(np.isfinite(command_bounds)): + raise ValueError(f"G1 walk velocity limits must contain finite lower/upper (vx, vy, wz), got {command_bounds}") + if np.any(command_bounds[0] > 0.0) or np.any(command_bounds[1] < 0.0): + raise ValueError("G1 walk velocity command range must contain zero on every axis") + if np.any(command_bounds[0] > command_bounds[1]): + raise ValueError("G1 walk velocity command lower bounds must not exceed upper bounds") + if not np.isfinite(command_cfg.gait_period) or command_cfg.gait_period <= 0.0: + raise ValueError(f"G1 walk gait period must be positive and finite, got {command_cfg.gait_period}") + + policy_obs = cfg.observations.policy + if policy_obs.command.command_name != _COMMAND_TERM_NAME: + raise ValueError( + f"G1 walk policy command observation must read {_COMMAND_TERM_NAME!r}, " + f"got {policy_obs.command.command_name!r}" + ) + if policy_obs.actions.action_name != _ACTION_TERM_NAME: + raise ValueError( + f"G1 walk previous-action observation must read {_ACTION_TERM_NAME!r}, " + f"got {policy_obs.actions.action_name!r}" + ) + if (policy_obs.sin_phase.offset, policy_obs.sin_phase.size) != (0, 2): + raise ValueError("G1 walk sine-phase observation must select offset=0, size=2") + if (policy_obs.cos_phase.offset, policy_obs.cos_phase.size) != (2, 2): + raise ValueError("G1 walk cosine-phase observation must select offset=2, size=2") + + task_values = { + "action_scale": np.asarray(action_state.action_scales, dtype=np.float32).tolist(), + "kp": kp.tolist(), + "kd": kd.tolist(), + "action_lower": np.asarray(env.action_space.low, dtype=np.float32).tolist(), + "action_upper": np.asarray(env.action_space.high, dtype=np.float32).tolist(), + "command_lower": command_bounds[0].tolist(), + "command_upper": command_bounds[1].tolist(), + "base_angular_velocity_scale": float(policy_obs.base_ang_vel.scale), + "joint_position_scale": float(policy_obs.joint_pos.scale), + "joint_velocity_scale": float(policy_obs.joint_vel.scale), + "gait_period_s": float(command_cfg.gait_period), + "gait_phase_offsets_rad": list(_GAIT_PHASE_OFFSETS_RAD), + "standing_command_threshold": _STANDING_COMMAND_THRESHOLD, + } + _require_task_schema_fields(task_values) + task = G1WalkTaskSpec(**task_values) + + return DeploymentProfile( + robot=robot, + task=task, + control=ControlSpec(period_s=cfg.ctrl_dt, state_timeout_s=0.1), + observation_size=observation_shape[0], + action_size=action_shape[0], + ) + + +def _single_joint_position_action_term(env: object) -> JointPositionActionTerm: + action_terms = getattr(env, "action_terms", None) + if not isinstance(action_terms, dict) or set(action_terms) != {_ACTION_TERM_NAME}: + names = tuple(action_terms) if isinstance(action_terms, dict) else action_terms + raise ValueError(f"G1 walk deployment requires exactly one {_ACTION_TERM_NAME!r} action term, got {names}") + action_term = action_terms[_ACTION_TERM_NAME] + if not isinstance(action_term, JointPositionActionTerm): + raise TypeError(f"Expected JointPositionActionTerm, got {type(action_term).__name__}") + return action_term + + +def _assert_vector(name: str, value: object, size: int, *, non_negative: bool = False) -> None: + vector = np.asarray(value) + if vector.shape != (size,) or not np.all(np.isfinite(vector)): + raise ValueError(f"{name} must contain {size} finite values, got shape {vector.shape}") + if non_negative and np.any(vector < 0.0): + raise ValueError(f"{name} must contain only non-negative values") + + +def _require_task_schema_fields(values: dict[str, object]) -> None: + """Stop before artifact creation while the reviewed TaskSpec is a shell.""" + + available = set(G1WalkTaskSpec.model_fields) + missing = set(values).difference(available) + if missing: + raise NotImplementedError( + "G1WalkTaskSpec must declare the profile fields before export can continue: " + ", ".join(sorted(missing)) + ) + + +__all__ = ["build_g1_walk_profile"] diff --git a/motrix_unitree_native/CMakeLists.txt b/motrix_unitree_native/CMakeLists.txt new file mode 100644 index 00000000..5790865f --- /dev/null +++ b/motrix_unitree_native/CMakeLists.txt @@ -0,0 +1,87 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +cmake_minimum_required(VERSION 3.18) +project(motrix_unitree_native LANGUAGES CXX) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +find_package(Python REQUIRED COMPONENTS Interpreter Development.Module) +find_package(pybind11 CONFIG REQUIRED) +find_package(Threads REQUIRED) + +set(UNITREE_SDK2_ROOT "" CACHE PATH "Unitree SDK2 source or installation root") +find_package(unitree_sdk2 CONFIG QUIET) + +if(NOT unitree_sdk2_FOUND) + if(NOT UNITREE_SDK2_ROOT) + if(DEFINED ENV{UNITREE_SDK2_ROOT}) + set(UNITREE_SDK2_ROOT "$ENV{UNITREE_SDK2_ROOT}") + else() + set(UNITREE_SDK2_ROOT "/opt/unitree_robotics") + endif() + endif() + + if(NOT EXISTS "${UNITREE_SDK2_ROOT}/include/unitree/robot/channel/channel_factory.hpp") + # CI and simulation-only installs must still resolve `--extra unitree`; the + # adapter imports the extension lazily and reports the missing backend. + message(WARNING + "Unitree SDK2 headers were not found under ${UNITREE_SDK2_ROOT}; building " + "motrix_unitree_native without the native DDS backend. Set UNITREE_SDK2_ROOT " + "and reinstall to enable hardware transport.") + return() + endif() + + if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64|amd64|AMD64") + set(UNITREE_ARCH "x86_64") + elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|arm64") + set(UNITREE_ARCH "aarch64") + else() + message(FATAL_ERROR "Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}") + endif() + + set(UNITREE_SDK2_LIBRARY "${UNITREE_SDK2_ROOT}/lib/${UNITREE_ARCH}/libunitree_sdk2.a") + set(UNITREE_DDS_LIBRARY "${UNITREE_SDK2_ROOT}/thirdparty/lib/${UNITREE_ARCH}/libddsc.so") + set(UNITREE_DDSXX_LIBRARY "${UNITREE_SDK2_ROOT}/thirdparty/lib/${UNITREE_ARCH}/libddscxx.so") + if(NOT EXISTS "${UNITREE_SDK2_LIBRARY}" OR NOT EXISTS "${UNITREE_DDS_LIBRARY}" OR + NOT EXISTS "${UNITREE_DDSXX_LIBRARY}") + message(FATAL_ERROR + "Unitree SDK2 libraries were not found for ${UNITREE_ARCH} under ${UNITREE_SDK2_ROOT}") + endif() + + # CycloneDDS C++ headers live under thirdparty/include/ddscxx (e.g. + # ddscxx/dds/topic/TopicTraits.hpp), the C headers under thirdparty/include. + add_library(motrix_unitree_sdk2 STATIC IMPORTED GLOBAL) + set_target_properties(motrix_unitree_sdk2 PROPERTIES + IMPORTED_LOCATION "${UNITREE_SDK2_LIBRARY}" + INTERFACE_INCLUDE_DIRECTORIES + "${UNITREE_SDK2_ROOT}/include;${UNITREE_SDK2_ROOT}/thirdparty/include;${UNITREE_SDK2_ROOT}/thirdparty/include/ddscxx") + target_link_directories(motrix_unitree_sdk2 INTERFACE + "${UNITREE_SDK2_ROOT}/thirdparty/lib/${UNITREE_ARCH}") + target_link_libraries(motrix_unitree_sdk2 INTERFACE ddscxx ddsc Threads::Threads dl) + set(UNITREE_TARGET motrix_unitree_sdk2) + set(UNITREE_RUNTIME_LIB_DIR "${UNITREE_SDK2_ROOT}/thirdparty/lib/${UNITREE_ARCH}") +else() + set(UNITREE_TARGET unitree_sdk2) +endif() + +pybind11_add_module(_native src/backend.cpp src/bindings.cpp) +target_link_libraries(_native PRIVATE ${UNITREE_TARGET}) +# DT_RPATH, not DT_RUNPATH: the robot shells source /opt/ros/foxy, whose +# LD_LIBRARY_PATH contains CycloneDDS 0.7.0. RUNPATH is searched after +# LD_LIBRARY_PATH, so the extension would bind to 0.7.0 and fail with +# "undefined symbol: ddsi_sertype_v0". RPATH is searched first. +target_link_options(_native PRIVATE "LINKER:--disable-new-dtags") + +if(DEFINED UNITREE_RUNTIME_LIB_DIR) + set_target_properties(_native PROPERTIES + BUILD_RPATH "${UNITREE_RUNTIME_LIB_DIR}" + INSTALL_RPATH "$ORIGIN;${UNITREE_RUNTIME_LIB_DIR}") +else() + # Installed SDK2 packages expose imported DDS targets. Ask CMake to carry + # their link directories into the extension's runtime search path. + set_target_properties(_native PROPERTIES INSTALL_RPATH_USE_LINK_PATH TRUE) +endif() +install(TARGETS _native LIBRARY DESTINATION motrix_unitree_native) diff --git a/motrix_unitree_native/README.md b/motrix_unitree_native/README.md new file mode 100644 index 00000000..133cca2c --- /dev/null +++ b/motrix_unitree_native/README.md @@ -0,0 +1,38 @@ +# Motrix Unitree Native + +This package builds the pybind11 extension used by the G1 `native_cpp` hardware +transport. It owns Unitree SDK2 DDS subscription/publication, LowState CRC and +mode validation, and the 1 kHz LowCmd watchdog writer. Python receives normalized +state snapshots and sends the latest policy command at policy-loop frequency. + +Build on a machine with CMake, pybind11, and Unitree SDK2 installed: + +```bash +UNITREE_SDK2_ROOT=/opt/unitree_robotics \ + python -m pip install ./motrix_unitree_native +``` + +The build embeds the SDK2 third-party library directory as a `DT_RPATH` entry, +which the dynamic loader searches **before** `LD_LIBRARY_PATH`. That matters on +robots whose shell sources ROS: `/opt/ros/foxy/lib/*/libddsc.so.0` is CycloneDDS +0.7 and does not export the 0.10 `ddsi_sertype_v0` API, so a `DT_RUNPATH` build +would bind to 0.7 and fail with `undefined symbol: ddsi_sertype_v0`. + +If the SDK is moved after installation, export its architecture-specific +library directory before starting deployment, for example: + +```bash +export LD_LIBRARY_PATH=/opt/unitree_robotics/thirdparty/lib/x86_64:$LD_LIBRARY_PATH +``` + +`G1Backend` owns the whole low-level contract: it converts the IMU quaternion to +xyzw and normalizes it, maps canonical joints to motor slots, validates the +LowState CRC and the `mode_pr`/`mode_machine` contract, publishes at +`command_period_s` with the CRC computed outside the writer lock, and damps +(kp=0, kd=`damping_kd`) whenever the state, the command or the mode contract is +stale. Mode/CRC problems are recoverable: the backend damps and reports +unhealthy, then resumes once valid frames return. + +For a source checkout of SDK2, set `UNITREE_SDK2_ROOT` to that checkout instead. +The native transport is G1 29-DOF specific; Go2 continues to use the existing +Python SDK transport. diff --git a/motrix_unitree_native/pyproject.toml b/motrix_unitree_native/pyproject.toml new file mode 100644 index 00000000..3d6f6fee --- /dev/null +++ b/motrix_unitree_native/pyproject.toml @@ -0,0 +1,15 @@ +[build-system] +requires = ["scikit-build-core>=0.10", "pybind11>=2.13", "numpy>=1.26"] +build-backend = "scikit_build_core.build" + +[project] +name = "motrix-unitree-native" +version = "0.4.0" +description = "Native Unitree SDK2 DDS transport for Motrix deployment" +requires-python = ">=3.10,<3.14" +dependencies = ["numpy>=1.26"] + +[tool.scikit-build] +wheel.packages = ["python/motrix_unitree_native"] +build-dir = "build/{wheel_tag}" +cmake.build-type = "Release" diff --git a/motrix_unitree_native/python/motrix_unitree_native/__init__.py b/motrix_unitree_native/python/motrix_unitree_native/__init__.py new file mode 100644 index 00000000..48d133de --- /dev/null +++ b/motrix_unitree_native/python/motrix_unitree_native/__init__.py @@ -0,0 +1,8 @@ +# Copyright Motphys Technology Co., Ltd. 2025, 2026 +# SPDX-License-Identifier: Apache-2.0 + +"""Python package for the optional Unitree SDK2 native extension.""" + +from ._native import G1Backend + +__all__ = ["G1Backend"] diff --git a/motrix_unitree_native/src/backend.cpp b/motrix_unitree_native/src/backend.cpp new file mode 100644 index 00000000..5d45311c --- /dev/null +++ b/motrix_unitree_native/src/backend.cpp @@ -0,0 +1,419 @@ +// Copyright Motphys Technology Co., Ltd. 2025, 2026 +// SPDX-License-Identifier: Apache-2.0 + +#include "backend.hpp" + +#include +#include +#include +#include + +#include + +namespace motrix_unitree_native { + +namespace { +constexpr int kMotorCount = 29; +constexpr int kMotorSlotCount = 35; +constexpr float kPositionStop = 0.0F; +constexpr float kVelocityStop = 0.0F; +// One dropped DDS sample is a hiccup, not a failure: only a sustained run of +// failed publishes makes the backend unhealthy. +constexpr int kPublishFailureLimit = 20; +} // namespace + +G1Backend::G1Backend( + std::string network_interface, int domain_id, std::string lowcmd_topic, + std::string lowstate_topic, std::vector motor_indices, int mode_pr, + int expected_mode_machine, int motor_mode, double damping_kd, + double command_period_s, double command_timeout_s, double state_timeout_s, + bool validate_crc, int subscriber_queue_depth) + : network_interface_(std::move(network_interface)), + domain_id_(domain_id), + lowcmd_topic_(std::move(lowcmd_topic)), + lowstate_topic_(std::move(lowstate_topic)), + mode_pr_(mode_pr), + expected_mode_machine_(expected_mode_machine), + motor_mode_(motor_mode), + damping_kd_(static_cast(damping_kd)), + command_period_(std::chrono::duration_cast( + std::chrono::duration(command_period_s))), + command_timeout_(std::chrono::duration_cast( + std::chrono::duration(command_timeout_s))), + state_timeout_(std::chrono::duration_cast( + std::chrono::duration(state_timeout_s))), + validate_crc_(validate_crc), + subscriber_queue_depth_(subscriber_queue_depth) { + if (motor_indices.size() != kMotorCount) { + throw std::invalid_argument("G1 native backend requires exactly 29 motor indices"); + } + std::copy(motor_indices.begin(), motor_indices.end(), motor_indices_.begin()); + std::array seen{}; + for (const int index : motor_indices_) { + if (index < 0 || index >= kMotorSlotCount || seen[static_cast(index)]) { + throw std::invalid_argument("G1 motor indices must be unique values in [0, 34]"); + } + seen[static_cast(index)] = true; + } + if (command_period_ <= std::chrono::nanoseconds::zero() || + command_timeout_ <= std::chrono::nanoseconds::zero() || + state_timeout_ <= std::chrono::nanoseconds::zero()) { + throw std::invalid_argument("native backend timing values must be positive"); + } + if (!std::isfinite(damping_kd) || damping_kd_ <= 0.0F) { + throw std::invalid_argument("native backend damping_kd must be positive and finite"); + } +} + +G1Backend::~G1Backend() { close(); } + +std::int64_t G1Backend::now_ns() { + return std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()) + .count(); +} + +std::uint32_t G1Backend::crc32_core(std::uint32_t* ptr, std::uint32_t length) { + // Unitree wire CRC: polynomial 0x04c11db7, init 0xFFFFFFFF, over + // (sizeof(message) >> 2) - 1 little-endian words (the trailing crc word is + // excluded). Implemented here so the build does not depend on an optional + // SDK header. + std::uint32_t crc = 0xFFFFFFFFU; + constexpr std::uint32_t kPolynomial = 0x04c11db7U; + for (std::uint32_t i = 0; i < length; ++i) { + std::uint32_t data = ptr[i]; + std::uint32_t mask = 1U << 31; + for (int bit = 0; bit < 32; ++bit) { + if ((crc & 0x80000000U) != 0U) { + crc = (crc << 1) ^ kPolynomial; + } else { + crc <<= 1; + } + if ((data & mask) != 0U) { + crc ^= kPolynomial; + } + mask >>= 1; + } + } + return crc; +} + +void G1Backend::open(double connect_timeout_s) { + if (connect_timeout_s <= 0.0 || !std::isfinite(connect_timeout_s)) { + throw std::invalid_argument("connect_timeout_s must be positive and finite"); + } + { + std::lock_guard lock(mutex_); + if (opened_ && !closed_) { + throw std::runtime_error("native Unitree backend is already open"); + } + unitree::robot::ChannelFactory::Instance()->Init(domain_id_, network_interface_); + subscriber_ = std::make_shared(lowstate_topic_); + subscriber_->InitChannel( + [this](const void* message) { lowstate_handler(message); }, subscriber_queue_depth_); + publisher_ = std::make_shared(lowcmd_topic_); + publisher_->InitChannel(); + initialize_command_locked(); + opened_ = true; + closed_ = false; + stopped_ = false; + enabled_ = false; + state_error_.clear(); + publish_error_.clear(); + publish_failures_ = 0; + have_state_ = false; + mode_ok_ = false; + invalid_mode_at_connect_ = false; + sequence_ = 0; + last_command_update_ns_ = now_ns(); + } + + std::unique_lock lock(mutex_); + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::duration_cast( + std::chrono::duration(connect_timeout_s)); + while (!have_state_ && !invalid_mode_at_connect_) { + if (state_condition_.wait_until(lock, deadline) == std::cv_status::timeout) { + lock.unlock(); + close(); + throw NativeTimeoutError("no valid G1 LowState received before connect timeout"); + } + } + if (invalid_mode_at_connect_) { + lock.unlock(); + close(); + throw std::runtime_error("G1 LowState mode_pr/mode_machine does not match the configured contract"); + } + lock.unlock(); + + writer_stop_.store(false); + writer_thread_ = std::thread(&G1Backend::command_writer_loop, this); +} + +void G1Backend::lowstate_handler(const void* message) { + const auto* incoming = static_cast(message); + if (incoming == nullptr) return; + LowState state = *incoming; + if (validate_crc_ && !valid_crc(state)) return; + if (!valid_mode(state)) { + // Recoverable: the robot may return to low-level mode. Keep publishing + // damping while unhealthy and clear this on the next contract-valid frame. + std::lock_guard lock(mutex_); + mode_ok_ = false; + if (!have_state_) invalid_mode_at_connect_ = true; + state_error_ = "G1 LowState mode_pr/mode_machine does not match the configured contract"; + state_condition_.notify_all(); + return; + } + + StateSnapshot snapshot; + snapshot.receive_time_ns = now_ns(); + for (int i = 0; i < kMotorCount; ++i) { + const auto motor_index = static_cast(motor_indices_[static_cast(i)]); + snapshot.joint_position[static_cast(i)] = state.motor_state()[motor_index].q(); + snapshot.joint_velocity[static_cast(i)] = state.motor_state()[motor_index].dq(); + } + const auto& q = state.imu_state().quaternion(); + const float w = q[0]; + const float x = q[1]; + const float y = q[2]; + const float z = q[3]; + const float norm = std::sqrt(w * w + x * x + y * y + z * z); + if (!std::isfinite(norm) || norm <= 1.0e-8F) { + // Drop the sample instead of latching an error: one corrupt IMU frame must + // not end the session. + std::lock_guard lock(mutex_); + state_error_ = "G1 LowState contains an invalid IMU quaternion"; + state_condition_.notify_all(); + return; + } + snapshot.quaternion_xyzw = {x / norm, y / norm, z / norm, w / norm}; + for (int i = 0; i < 3; ++i) { + snapshot.gyro[static_cast(i)] = state.imu_state().gyroscope()[i]; + snapshot.accelerometer[static_cast(i)] = state.imu_state().accelerometer()[i]; + } + const auto& remote = state.wireless_remote(); + const auto remote_count = std::min(snapshot.wireless_remote.size(), remote.size()); + for (std::size_t i = 0; i < remote_count; ++i) snapshot.wireless_remote[i] = remote[i]; + + { + std::lock_guard lock(mutex_); + snapshot.sequence = ++sequence_; + latest_state_ = snapshot; + have_state_ = true; + mode_ok_ = true; + state_error_.clear(); + last_state_receive_ns_ = snapshot.receive_time_ns; + last_communication_ns_ = snapshot.receive_time_ns; + } + state_condition_.notify_all(); +} + +bool G1Backend::valid_crc(const LowState& state) const { + auto copy = state; + return state.crc() == crc32_core(reinterpret_cast(©), + (sizeof(LowState) >> 2U) - 1U); +} + +bool G1Backend::valid_mode(const LowState& state) const { + if (mode_pr_ >= 0 && state.mode_pr() != mode_pr_) return false; + return expected_mode_machine_ < 0 || state.mode_machine() == expected_mode_machine_; +} + +StateSnapshot G1Backend::read_state(double timeout_s, std::int64_t after_sequence) { + if (timeout_s <= 0.0 || !std::isfinite(timeout_s)) { + throw std::invalid_argument("state timeout must be positive and finite"); + } + std::unique_lock lock(mutex_); + const auto deadline = std::chrono::steady_clock::now() + + std::chrono::duration_cast( + std::chrono::duration(timeout_s)); + const auto fresh = [this, after_sequence] { + return have_state_ && + (after_sequence < 0 || static_cast(latest_state_.sequence) > after_sequence); + }; + while (!fresh()) { + if (state_condition_.wait_until(lock, deadline) == std::cv_status::timeout) { + std::string message = "no fresh G1 LowState received before timeout"; + const std::string& reason = !state_error_.empty() ? state_error_ : publish_error_; + if (!reason.empty()) message += ": " + reason; + throw NativeTimeoutError(message); + } + } + return latest_state_; +} + +void G1Backend::write_command(const std::array& position, + const std::array& velocity, + const std::array& torque, + const std::array& kp, + const std::array& kd) { + std::lock_guard lock(mutex_); + if (!opened_ || closed_) throw std::runtime_error("native Unitree backend is not open"); + for (int i = 0; i < kMotorCount; ++i) { + auto& motor = command_.motor_cmd()[static_cast(motor_indices_[static_cast(i)])]; + motor.mode() = motor_mode_; + motor.q() = position[static_cast(i)]; + motor.dq() = velocity[static_cast(i)]; + motor.kp() = kp[static_cast(i)]; + motor.kd() = kd[static_cast(i)]; + motor.tau() = torque[static_cast(i)]; + } + enabled_ = true; + stopped_ = false; + last_command_update_ns_ = now_ns(); +} + +void G1Backend::write_torque(const std::array& torque) { + std::array zeros{}; + write_command(zeros, zeros, torque, zeros, zeros); +} + +void G1Backend::set_enabled(bool enabled) { + std::lock_guard lock(mutex_); + enabled_ = enabled; + stopped_ = false; + if (enabled) { + // Never publish a kp=0/kd=0 frame between enabling and the first command. + set_damping_locked(); + } + last_command_update_ns_ = now_ns(); +} + +void G1Backend::stop() { + LowCmd outgoing; + { + std::lock_guard lock(mutex_); + enabled_ = false; + stopped_ = true; + set_damping_locked(); + last_command_update_ns_ = now_ns(); + outgoing = command_; + } + // Publish the first damping frame synchronously. The periodic writer keeps + // repeating it afterwards, but shutdown should not depend on its next tick. + publish(outgoing); +} + +void G1Backend::close() { + LowCmd outgoing; + bool publish_damping = false; + { + std::lock_guard lock(mutex_); + if (closed_) return; + if (opened_) { + enabled_ = false; + stopped_ = true; + set_damping_locked(); + outgoing = command_; + publish_damping = true; + } + writer_stop_.store(true); + } + if (publish_damping) publish(outgoing); + if (writer_thread_.joinable()) writer_thread_.join(); + std::lock_guard lock(mutex_); + publisher_.reset(); + subscriber_.reset(); + opened_ = false; + closed_ = true; +} + +bool G1Backend::healthy() const { + std::lock_guard lock(mutex_); + if (!opened_ || closed_ || stopped_) return false; + if (!state_error_.empty() || !publish_error_.empty()) return false; + return have_state_ && mode_ok_ && now_ns() - last_state_receive_ns_ <= state_timeout_.count(); +} + +std::string G1Backend::health_reason() const { + std::lock_guard lock(mutex_); + if (!publish_error_.empty()) return publish_error_; + if (!state_error_.empty()) return state_error_; + if (closed_) return "backend is closed"; + if (!opened_) return "backend is not open"; + if (stopped_) return "backend is stopped"; + if (!have_state_ || now_ns() - last_state_receive_ns_ > state_timeout_.count()) return "LowState is stale"; + return ""; +} + +std::int64_t G1Backend::last_communication_ns() const { + std::lock_guard lock(mutex_); + return last_communication_ns_; +} + +void G1Backend::initialize_command_locked() { + if (mode_pr_ >= 0) command_.mode_pr() = mode_pr_; + if (expected_mode_machine_ >= 0) command_.mode_machine() = expected_mode_machine_; + for (std::size_t i = 0; i < command_.motor_cmd().size(); ++i) { + auto& motor = command_.motor_cmd()[i]; + motor.mode() = 0; + motor.q() = kPositionStop; + motor.dq() = kVelocityStop; + motor.kp() = 0.0F; + motor.kd() = 0.0F; + motor.tau() = 0.0F; + } + for (const int index : motor_indices_) { + auto& motor = command_.motor_cmd()[static_cast(index)]; + motor.mode() = motor_mode_; + // The first frame published after open()/enable() must already damp. + motor.kd() = damping_kd_; + } +} + +void G1Backend::set_damping_locked() { + for (int i = 0; i < kMotorCount; ++i) { + const auto index = static_cast(motor_indices_[static_cast(i)]); + auto& motor = command_.motor_cmd()[index]; + motor.mode() = motor_mode_; + motor.q() = have_state_ ? latest_state_.joint_position[static_cast(i)] : 0.0F; + motor.dq() = 0.0F; + motor.kp() = 0.0F; + motor.kd() = damping_kd_; + motor.tau() = 0.0F; + } +} + +bool G1Backend::publish(const LowCmd& command) { + const std::shared_ptr publisher = publisher_; + if (!publisher) return false; + LowCmd message = command; + message.crc() = crc32_core(reinterpret_cast(&message), + (sizeof(LowCmd) >> 2U) - 1U); + const bool ok = publisher->Write(message, 0); + std::lock_guard lock(mutex_); + if (ok) { + last_communication_ns_ = now_ns(); + publish_failures_ = 0; + publish_error_.clear(); + } else if (++publish_failures_ >= kPublishFailureLimit) { + publish_error_ = "G1 LowCmd DDS publish failed"; + } + return ok; +} + +void G1Backend::command_writer_loop() { + auto next = std::chrono::steady_clock::now(); + while (!writer_stop_.load()) { + next += command_period_; + LowCmd outgoing; + { + std::lock_guard lock(mutex_); + const auto now = now_ns(); + const bool state_stale = !have_state_ || now - last_state_receive_ns_ > state_timeout_.count(); + const bool command_stale = now - last_command_update_ns_ > command_timeout_.count(); + if (!enabled_ || stopped_ || !mode_ok_ || state_stale || command_stale) { + set_damping_locked(); + } + outgoing = command_; + } + // CRC and the DDS write happen outside the lock: the policy loop and + // read_state() must never wait on transport I/O. + publish(outgoing); + std::this_thread::sleep_until(next); + if (std::chrono::steady_clock::now() > next + command_period_) next = std::chrono::steady_clock::now(); + } +} + +} // namespace motrix_unitree_native diff --git a/motrix_unitree_native/src/backend.hpp b/motrix_unitree_native/src/backend.hpp new file mode 100644 index 00000000..ed7c4e78 --- /dev/null +++ b/motrix_unitree_native/src/backend.hpp @@ -0,0 +1,127 @@ +// Copyright Motphys Technology Co., Ltd. 2025, 2026 +// SPDX-License-Identifier: Apache-2.0 + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace motrix_unitree_native { + +/// Raised when no fresh LowState (or no first LowState) arrives in time. +/// Registered as a subclass of Python ``TimeoutError``. +class NativeTimeoutError : public std::runtime_error { + public: + explicit NativeTimeoutError(const std::string& message) : std::runtime_error(message) {} +}; + +struct StateSnapshot { + std::uint64_t sequence = 0; + std::int64_t receive_time_ns = 0; + std::array joint_position{}; + std::array joint_velocity{}; + std::array quaternion_xyzw{}; + std::array gyro{}; + std::array accelerometer{}; + std::array wireless_remote{}; +}; + +class G1Backend { + public: + G1Backend( + std::string network_interface, int domain_id, std::string lowcmd_topic, + std::string lowstate_topic, std::vector motor_indices, int mode_pr, + int expected_mode_machine, int motor_mode, double damping_kd, + double command_period_s, double command_timeout_s, double state_timeout_s, + bool validate_crc, int subscriber_queue_depth); + ~G1Backend(); + + void open(double connect_timeout_s); + StateSnapshot read_state(double timeout_s, std::int64_t after_sequence); + void write_command(const std::array& position, + const std::array& velocity, + const std::array& torque, + const std::array& kp, + const std::array& kd); + void write_torque(const std::array& torque); + void set_enabled(bool enabled); + void stop(); + void close(); + + bool healthy() const; + std::string health_reason() const; + std::int64_t last_communication_ns() const; + + private: + using LowCmd = unitree_hg::msg::dds_::LowCmd_; + using LowState = unitree_hg::msg::dds_::LowState_; + using Publisher = unitree::robot::ChannelPublisher; + using Subscriber = unitree::robot::ChannelSubscriber; + + static std::int64_t now_ns(); + static std::uint32_t crc32_core(std::uint32_t* ptr, std::uint32_t length); + void lowstate_handler(const void* message); + void command_writer_loop(); + void initialize_command_locked(); + void set_damping_locked(); + /// CRC and publish one command without holding mutex_. + bool publish(const LowCmd& command); + bool valid_crc(const LowState& state) const; + bool valid_mode(const LowState& state) const; + + std::string network_interface_; + int domain_id_; + std::string lowcmd_topic_; + std::string lowstate_topic_; + std::array motor_indices_{}; + int mode_pr_; + int expected_mode_machine_; + int motor_mode_; + float damping_kd_; + std::chrono::nanoseconds command_period_; + std::chrono::nanoseconds command_timeout_; + std::chrono::nanoseconds state_timeout_; + bool validate_crc_; + int subscriber_queue_depth_; + + std::shared_ptr publisher_; + std::shared_ptr subscriber_; + LowCmd command_{}; + + mutable std::mutex mutex_; + std::condition_variable state_condition_; + StateSnapshot latest_state_{}; + bool have_state_ = false; + bool mode_ok_ = false; + bool invalid_mode_at_connect_ = false; + std::uint64_t sequence_ = 0; + std::int64_t last_state_receive_ns_ = 0; + std::int64_t last_command_update_ns_ = 0; + std::int64_t last_communication_ns_ = 0; + std::string state_error_; + std::string publish_error_; + int publish_failures_ = 0; + bool enabled_ = false; + bool stopped_ = false; + bool opened_ = false; + bool closed_ = false; + + std::atomic writer_stop_{false}; + std::thread writer_thread_; +}; + +} // namespace motrix_unitree_native diff --git a/motrix_unitree_native/src/bindings.cpp b/motrix_unitree_native/src/bindings.cpp new file mode 100644 index 00000000..1321a2bd --- /dev/null +++ b/motrix_unitree_native/src/bindings.cpp @@ -0,0 +1,119 @@ +// Copyright Motphys Technology Co., Ltd. 2025, 2026 +// SPDX-License-Identifier: Apache-2.0 + +#include "backend.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace py = pybind11; +using motrix_unitree_native::G1Backend; +using motrix_unitree_native::NativeTimeoutError; +using motrix_unitree_native::StateSnapshot; + +namespace { + +template +std::array array_from_numpy( + const py::array_t& value, + const char* name) { + if (value.ndim() != 1 || static_cast(value.shape(0)) != N) { + throw std::invalid_argument(std::string(name) + " has an unexpected shape"); + } + std::array result{}; + std::memcpy(result.data(), value.data(), sizeof(float) * N); + for (const float item : result) { + if (!std::isfinite(item)) throw std::invalid_argument(std::string(name) + " contains a non-finite value"); + } + return result; +} + +py::array_t array_to_numpy(const float* data, std::size_t count) { + py::array_t result(count); + std::memcpy(result.mutable_data(), data, sizeof(float) * count); + return result; +} + +py::dict snapshot_to_dict(const StateSnapshot& snapshot) { + py::dict result; + result["sequence"] = snapshot.sequence; + result["receive_time_ns"] = snapshot.receive_time_ns; + result["joint_position"] = array_to_numpy(snapshot.joint_position.data(), snapshot.joint_position.size()); + result["joint_velocity"] = array_to_numpy(snapshot.joint_velocity.data(), snapshot.joint_velocity.size()); + result["quaternion_xyzw"] = array_to_numpy(snapshot.quaternion_xyzw.data(), snapshot.quaternion_xyzw.size()); + result["gyro"] = array_to_numpy(snapshot.gyro.data(), snapshot.gyro.size()); + result["accelerometer"] = array_to_numpy(snapshot.accelerometer.data(), snapshot.accelerometer.size()); + result["wireless_remote"] = py::bytes( + reinterpret_cast(snapshot.wireless_remote.data()), snapshot.wireless_remote.size()); + return result; +} + +} // namespace + +PYBIND11_MODULE(_native, module) { + module.doc() = "Native Unitree SDK2 transport for Motrix deployment"; + + // Keep the Python contract explicit: callers can catch builtins.TimeoutError. + py::register_exception(module, "TimeoutError", PyExc_TimeoutError); + + py::class_(module, "G1Backend") + .def( + py::init, int, int, int, double, double, + double, double, bool, int>(), + py::arg("network_interface"), py::arg("domain_id"), py::arg("lowcmd_topic"), + py::arg("lowstate_topic"), py::arg("motor_indices"), py::arg("mode_pr") = -1, + py::arg("expected_mode_machine") = -1, py::arg("motor_mode") = 1, + py::arg("damping_kd") = 3.0, py::arg("command_period_s") = 0.001, + py::arg("command_timeout_s") = 0.1, py::arg("state_timeout_s") = 0.1, + py::arg("validate_crc") = true, py::arg("subscriber_queue_depth") = 10) + .def("open", [](G1Backend& backend, double connect_timeout_s) { + py::gil_scoped_release release; + backend.open(connect_timeout_s); + }, py::arg("connect_timeout_s") = 5.0) + .def("read_state", [](G1Backend& backend, double timeout_s, std::int64_t after_sequence) { + StateSnapshot snapshot; + { + py::gil_scoped_release release; + snapshot = backend.read_state(timeout_s, after_sequence); + } + return snapshot_to_dict(snapshot); + }, py::arg("timeout_s") = 0.1, py::arg("after_sequence") = -1) + .def("write_command", []( + G1Backend& backend, + const py::array_t& position, + const py::array_t& velocity, + const py::array_t& torque, + const py::array_t& kp, + const py::array_t& kd) { + backend.write_command( + array_from_numpy<29>(position, "joint_position"), array_from_numpy<29>(velocity, "joint_velocity"), + array_from_numpy<29>(torque, "feedforward_torque"), array_from_numpy<29>(kp, "kp"), + array_from_numpy<29>(kd, "kd")); + }, py::arg("joint_position"), py::arg("joint_velocity"), py::arg("feedforward_torque"), py::arg("kp"), + py::arg("kd")) + .def("write_torque", [](G1Backend& backend, + const py::array_t& torque) { + backend.write_torque(array_from_numpy<29>(torque, "torque")); + }) + .def("set_enabled", &G1Backend::set_enabled, py::arg("enabled")) + .def("stop", [](G1Backend& backend) { + py::gil_scoped_release release; + backend.stop(); + }) + .def("close", [](G1Backend& backend) { + py::gil_scoped_release release; + backend.close(); + }) + .def("health", [](const G1Backend& backend) { + py::dict result; + result["healthy"] = backend.healthy(); + result["reason"] = backend.health_reason(); + result["last_successful_communication_ns"] = backend.last_communication_ns(); + return result; + }); +} diff --git a/pyproject.toml b/pyproject.toml index 89367e5d..4187605a 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -22,7 +22,7 @@ dependencies = [ rslrl = ["motrix-rl-rslrl==0.4.0"] skrl-jax = ["motrix-rl-skrl[jax]==0.4.0"] unitree = [ - "motrix-deploy-unitree==0.4.0", + "motrix-deploy-unitree[native]==0.4.0", ] docs = [ "sphinx", @@ -70,6 +70,7 @@ members = [ "motrix_deploy_mujoco", "motrix_deploy_motrixsim", "motrix_deploy_unitree", + "motrix_unitree_native", "motrix_deploy_tasks", "motrix_env_core", "motrix_env_motrixsim", @@ -102,6 +103,7 @@ motrix-deploy = { workspace = true } motrix-deploy-mujoco = { workspace = true } motrix-deploy-motrixsim = { workspace = true } motrix-deploy-unitree = { workspace = true } +motrix-unitree-native = { workspace = true } motrix-deploy-tasks = { workspace = true } motrix-env-motrixsim = { workspace = true } motrix-env-mujoco = { workspace = true } diff --git a/tests/test_workspace_versions.py b/tests/test_workspace_versions.py index 627601bd..9e82c464 100644 --- a/tests/test_workspace_versions.py +++ b/tests/test_workspace_versions.py @@ -3,7 +3,9 @@ """Workspace release version contracts.""" +import ast import re +from importlib import metadata from pathlib import Path from packaging.requirements import Requirement @@ -18,7 +20,7 @@ def _project_field(path: Path, field: str) -> str: return match.group(1) -def _workspace_pyprojects() -> list[tuple[Path, str, str]]: +def _workspace_member_dirs() -> list[Path]: root_pyproject = ROOT / "pyproject.toml" members_match = re.search( r"\[tool\.uv\.workspace\]\s*members\s*=\s*\[(.*?)\]", @@ -26,7 +28,11 @@ def _workspace_pyprojects() -> list[tuple[Path, str, str]]: re.DOTALL, ) assert members_match is not None, "Missing [tool.uv.workspace] members in root pyproject.toml" - members = re.findall(r'"([^"]+)"', members_match.group(1)) + return [ROOT / member for member in re.findall(r'"([^"]+)"', members_match.group(1))] + + +def _workspace_pyprojects() -> list[tuple[Path, str, str]]: + root_pyproject = ROOT / "pyproject.toml" projects = [ ( root_pyproject, @@ -34,8 +40,8 @@ def _workspace_pyprojects() -> list[tuple[Path, str, str]]: _project_field(root_pyproject, "version"), ) ] - for member in members: - pyproject = ROOT / member / "pyproject.toml" + for member in _workspace_member_dirs(): + pyproject = member / "pyproject.toml" projects.append((pyproject, _project_field(pyproject, "name"), _project_field(pyproject, "version"))) return projects @@ -99,3 +105,65 @@ def test_workspace_member_dependencies_are_pinned_to_workspace_version(): f"{pyproject} must pin {requirement.name} to the workspace version " f"{version}; got {requirement.specifier}" ) + + +def _declared_source_versions() -> list[tuple[Path, str, ast.expr]]: + """(initializer, distribution name, assigned value) for every package declaring ``__version__``.""" + declared = [] + for member in _workspace_member_dirs(): + distribution = _project_field(member / "pyproject.toml", "name") + for initializer in sorted(member.glob("src/*/__init__.py")): + for node in ast.parse(initializer.read_text()).body: + if not isinstance(node, ast.Assign): + continue + if any(isinstance(target, ast.Name) and target.id == "__version__" for target in node.targets): + declared.append((initializer, distribution, node.value)) + return declared + + +def _installed_version(distribution: str) -> str | None: + try: + return metadata.version(distribution) + except metadata.PackageNotFoundError: + return None + + +def test_source_versions_track_the_installed_distribution(): + """``__version__`` must come from installed metadata, never a copied literal. + + ``cz bump`` only rewrites files matched by the root ``version_files`` + patterns, which do not cover package initializers, so a hardcoded literal + drifts silently from the release version. + """ + projects = _workspace_projects() + declared = _declared_source_versions() + assert declared, "Expected at least one workspace package to declare __version__" + + for initializer, distribution, value in declared: + assert isinstance(value, ast.Call), ( + f"{initializer} must derive __version__ from installed metadata, got {ast.unparse(value)}" + ) + # Accept both `version(...)` after `from importlib.metadata import version` + # and the module-qualified `metadata.version(...)`; upstream uses the former. + reader = value.func.id if isinstance(value.func, ast.Name) else getattr(value.func, "attr", None) + assert reader == "version", f"{initializer} must read the metadata version function, got {ast.unparse(value)}" + assert len(value.args) == 1, ( + f"{initializer} must read metadata.version(), got {ast.unparse(value)}" + ) + assert isinstance(value.args[0], ast.Constant), ( + f"{initializer} must pass a literal distribution name, got {ast.unparse(value)}" + ) + assert value.args[0].value == distribution, ( + f"{initializer} reads the metadata version of {value.args[0].value!r}, " + f"but its distribution is {distribution!r}" + ) + installed = _installed_version(distribution) + if installed is None: + # Not every workspace package is installed in every environment + # (e.g. the Unitree plugin is Python 3.10 only), so the installed + # version can only be checked where the distribution is present. + continue + assert installed == projects[distribution], ( + f"{distribution} is installed as {installed!r}, expected the workspace version " + f"{projects[distribution]!r}; reinstall it with the matching install command" + ) diff --git a/uv.lock b/uv.lock index 50dd5a1a..4573fb56 100644 --- a/uv.lock +++ b/uv.lock @@ -116,6 +116,7 @@ members = [ "motrix-rl-rslrl", "motrix-rl-skrl", "motrix-robots", + "motrix-unitree-native", "motrixlab", ] @@ -1439,14 +1440,21 @@ dependencies = [ { name = "unitree-sdk2py" }, ] +[package.optional-dependencies] +native = [ + { name = "motrix-unitree-native" }, +] + [package.metadata] requires-dist = [ { name = "hydra-core", specifier = ">=1.3,<1.4" }, { name = "motrix-deploy", editable = "motrix_deploy" }, { name = "motrix-env-core", editable = "motrix_env_core" }, + { name = "motrix-unitree-native", marker = "extra == 'native'", editable = "motrix_unitree_native" }, { name = "numpy", specifier = ">=1.26" }, { name = "unitree-sdk2py", git = "https://github.com/motphys-developers/unitree_sdk2_python?rev=ff87e731174f7258e0d09980897c55116634939a" }, ] +provides-extras = ["native"] [[package]] name = "motrix-env-core" @@ -1608,7 +1616,7 @@ tbb = [ { name = "motrix-env-core", extra = ["tbb"] }, ] unitree = [ - { name = "motrix-deploy-unitree" }, + { name = "motrix-deploy-unitree", extra = ["native"] }, ] [package.dev-dependencies] @@ -1636,7 +1644,7 @@ requires-dist = [ { name = "motrix-deploy-motrixsim", editable = "motrix_deploy_motrixsim" }, { name = "motrix-deploy-mujoco", editable = "motrix_deploy_mujoco" }, { name = "motrix-deploy-tasks", editable = "motrix_deploy_tasks" }, - { name = "motrix-deploy-unitree", marker = "extra == 'unitree'", editable = "motrix_deploy_unitree" }, + { name = "motrix-deploy-unitree", extras = ["native"], marker = "extra == 'unitree'", editable = "motrix_deploy_unitree" }, { name = "motrix-env-core", extras = ["tbb"], marker = "extra == 'tbb'", editable = "motrix_env_core" }, { name = "motrix-env-motrixsim", editable = "motrix_env_motrixsim" }, { name = "motrix-env-mujoco", editable = "motrix_env_mujoco" }, @@ -1814,6 +1822,17 @@ dependencies = [ [package.metadata] requires-dist = [{ name = "motrix-env-core", editable = "motrix_env_core" }] +[[package]] +name = "motrix-unitree-native" +version = "0.4.0" +source = { editable = "motrix_unitree_native" } +dependencies = [ + { name = "numpy" }, +] + +[package.metadata] +requires-dist = [{ name = "numpy", specifier = ">=1.26" }] + [[package]] name = "motrixlab" version = "0.4.0"