From d8936f3e414b4fdfe61ac42830992ca8f20bf06c Mon Sep 17 00:00:00 2001 From: johnnynunez Date: Mon, 20 Jul 2026 03:50:42 +0200 Subject: [PATCH 1/6] feat: add reBot DevArm RobStride support Add a generic environment runtime lifecycle, a guarded motorbridge SocketCAN backend for the seven-motor B601-RS arm, passive hardware validation, documentation, and unit tests. --- README.md | 2 + docs/source-en/index.rst | 1 + .../rst_source/development/add_robot.rst | 22 +- .../rst_source/development/architecture.rst | 19 +- .../rst_source/usage/rebot_robstride.rst | 119 +++++ pyproject.toml | 4 + robots/libero/__init__.py | 12 +- robots/libero/runtime.py | 171 ++++++ robots/rebot_robstride/__init__.py | 38 ++ robots/rebot_robstride/config.py | 195 +++++++ .../config/rebot_robstride.example.yaml | 29 + robots/rebot_robstride/driver.py | 363 +++++++++++++ robots/rebot_robstride/env_client.py | 50 ++ robots/rebot_robstride/env_server.py | 125 +++++ robots/rebot_robstride/prompt_bundle.py | 13 + robots/rebot_robstride/prompts/__init__.py | 1 + robots/rebot_robstride/prompts/system.py | 26 + robots/rebot_robstride/runtime.py | 67 +++ robots/rebot_robstride/toolkit.py | 134 +++++ rpent/cli/main.py | 500 ++++++------------ rpent/envs/__init__.py | 5 +- rpent/envs/base.py | 11 + rpent/envs/process.py | 113 ++++ rpent/envs/runtime.py | 19 + scripts/check_rebot_robstride.py | 36 ++ tests/envs/test_runtime_registry.py | 44 ++ tests/robots/rebot_robstride/test_config.py | 60 +++ tests/robots/rebot_robstride/test_driver.py | 276 ++++++++++ .../robots/rebot_robstride/test_env_client.py | 30 ++ .../robots/rebot_robstride/test_env_server.py | 64 +++ tests/robots/rebot_robstride/test_toolkit.py | 72 +++ tests/robots/test_runtimes.py | 90 ++++ 32 files changed, 2346 insertions(+), 365 deletions(-) create mode 100644 docs/source-en/rst_source/usage/rebot_robstride.rst create mode 100644 robots/libero/runtime.py create mode 100644 robots/rebot_robstride/__init__.py create mode 100644 robots/rebot_robstride/config.py create mode 100644 robots/rebot_robstride/config/rebot_robstride.example.yaml create mode 100644 robots/rebot_robstride/driver.py create mode 100644 robots/rebot_robstride/env_client.py create mode 100644 robots/rebot_robstride/env_server.py create mode 100644 robots/rebot_robstride/prompt_bundle.py create mode 100644 robots/rebot_robstride/prompts/__init__.py create mode 100644 robots/rebot_robstride/prompts/system.py create mode 100644 robots/rebot_robstride/runtime.py create mode 100644 robots/rebot_robstride/toolkit.py create mode 100644 rpent/envs/process.py create mode 100644 rpent/envs/runtime.py create mode 100644 scripts/check_rebot_robstride.py create mode 100644 tests/envs/test_runtime_registry.py create mode 100644 tests/robots/rebot_robstride/test_config.py create mode 100644 tests/robots/rebot_robstride/test_driver.py create mode 100644 tests/robots/rebot_robstride/test_env_client.py create mode 100644 tests/robots/rebot_robstride/test_env_server.py create mode 100644 tests/robots/rebot_robstride/test_toolkit.py create mode 100644 tests/robots/test_runtimes.py diff --git a/README.md b/README.md index eb32cac4..bd050020 100644 --- a/README.md +++ b/README.md @@ -75,6 +75,7 @@ RPent is built upon three core design principles: **service-oriented, standardiz @@ -177,6 +178,7 @@ See [SETUP_ROBOCASA.zh.md](docs/SETUP_ROBOCASA.zh.md) for the full RoboCasa365 + ## Documentation - [Adding a new environment](https://rpent.readthedocs.io/en/latest/rst_source/extending/new_env.html) — plug a new simulator / robot into the runner ([中文](https://rpent.readthedocs.io/zh-cn/latest/rst_source/extending/new_env.html)). +- [reBot DevArm (RobStride)](docs/source-en/rst_source/usage/rebot_robstride.rst) — safe SocketCAN setup, passive validation, and guarded motion tools. - [RoboCasa setup](docs/SETUP_ROBOCASA.zh.md) — RoboCasa365 + RLDX-1 install and run guide. - [`docs/`](docs/README.md) — local Sphinx build and preview instructions. diff --git a/docs/source-en/index.rst b/docs/source-en/index.rst index c2990c28..4975f68c 100644 --- a/docs/source-en/index.rst +++ b/docs/source-en/index.rst @@ -88,6 +88,7 @@ Welcome to RPent RoboCasa Franka SO-101 + reBot DevArm (RobStride) .. toctree:: :maxdepth: 2 diff --git a/docs/source-en/rst_source/development/add_robot.rst b/docs/source-en/rst_source/development/add_robot.rst index b61c2637..d3fa98e8 100644 --- a/docs/source-en/rst_source/development/add_robot.rst +++ b/docs/source-en/rst_source/development/add_robot.rst @@ -64,7 +64,8 @@ For a new env ``myenv``, the file layout is: .. code-block:: text robots/myenv/ - __init__.py # entry point — get_env_spec() / get_toolkit() factories + __init__.py # get_env_spec / get_runtime / get_toolkit factories + runtime.py # process/client lifecycle behind EnvRuntime env_client.py # MyEnvClient — agent-side RPC stub (§1) prompt_bundle.py # system()/user() prompt factories (§2) toolkit.py # MyEnvToolkit + primitives + tool schemas (§3) @@ -73,7 +74,7 @@ For a new env ``myenv``, the file layout is: ``__init__.py`` is the package's entry point. The registry in ``rpent/envs/base.py`` lazily imports ``robots.`` on demand and calls its -two factories: +three factories: .. code-block:: python @@ -85,6 +86,10 @@ two factories: def get_env_spec() -> EnvSpec: return EnvSpec(name="myenv", prompts=PromptBundle(system=system_prompt, user=user_prompt)) + def get_runtime(*, args, output_dir, dashboard=None): + from robots.myenv.runtime import MyEnvRuntime + return MyEnvRuntime(args=args, output_dir=output_dir, dashboard=dashboard) + def get_toolkit(*, primitives_kwargs: dict[str, Any], video_path: str | None = None): from robots.myenv.toolkit import MyEnvToolkit return MyEnvToolkit(primitives_kwargs=primitives_kwargs, video_path=video_path) @@ -150,13 +155,14 @@ Wrap the facade in a dispatcher and serve over ``SocketRpcServer``: print(json.dumps({"event": "transport_ready", "kind": "socket", "host": host, "port": bound_port}), flush=True) -The ``transport_ready`` event on stdout is required — -``start_env_server()`` in ``rpent/cli/main.py`` blocks until it sees it. +The ``transport_ready`` event on stdout is required — the selected +``EnvRuntime`` waits for it before constructing the client. -``rpent/cli/main.py`` currently imports ``LiberoEnvClient`` and the LIBERO env_server -script path directly. Adding a new env means either branching on -``args.env_name`` to pick the client class + driver script, or factoring those -two callsites out behind a per-env helper. +The environment's ``EnvRuntime`` owns server startup/attach, client +construction, and shutdown. ``rpent/cli/main.py`` calls ``get_runtime`` and +contains no robot-specific branching. See ``robots/libero/runtime.py`` for a +model-backed simulator and ``robots/rebot_robstride/runtime.py`` for a physical +robot without a VLA process. 2. ``prompt_bundle.py`` ----------------------- diff --git a/docs/source-en/rst_source/development/architecture.rst b/docs/source-en/rst_source/development/architecture.rst index 224cad0a..bb72c1f1 100644 --- a/docs/source-en/rst_source/development/architecture.rst +++ b/docs/source-en/rst_source/development/architecture.rst @@ -91,6 +91,7 @@ The code that implements the framework is split cleanly by concern: robots/ libero/ # LIBERO env_client / env_server / vla_server / # toolkit / prompt_bundle. The reference env. + rebot_robstride/ # Physical reBot driver over motorbridge + SocketCAN. (robocasa/) # RoboCasa driver (see scripts/run_robocasa.sh). (franka/) # Franka driver — in progress. (so101/) # SO-101 driver — in progress. @@ -105,15 +106,14 @@ The runner (``rpent/cli/main.py``) use day-to-day). 2. Creates the per-run scratch directory (``--output-dir`` or an auto-generated one under ``runs/``). -3. Spawns the **env_server** as a subprocess and waits for its - ``transport_ready`` JSON event on stdout. That event carries the - host/port the socket RPC is listening on; ``rpent/cli/main.py`` records - the endpoint under ``/`` so the client can find it. -4. Spawns (or attaches to) the **vla_server** the same way, using - ``--vla-endpoint`` when reusing a running instance. -5. Builds the **toolkit** for the chosen env via the env's - ``get_toolkit(primitives_kwargs=...)`` factory, wiring in the env - client and the VLA client. +3. Builds the chosen environment's **EnvRuntime** through + ``get_runtime(args=..., output_dir=...)``. +4. Calls ``runtime.start()``. The runtime starts or attaches to the processes + that environment needs, waits for ``transport_ready``, builds its clients, + and returns the environment toolkit. LIBERO starts env + VLA processes; + physical reBot control starts only its single-owner hardware server. +5. Keeps process/client details outside the runner, so adding a robot never + adds an ``if env_name == ...`` branch to ``main.py``. 6. Builds the **cerebrum** via ``rpent.cerebrum.base.build_cerebrum``, selecting one of ``api_loop.py`` / ``claude_code.py`` / ``codex.py`` based on ``--cerebrum``. @@ -137,6 +137,7 @@ factories the package exposes: # robots/myenv/__init__.py def get_env_spec() -> EnvSpec: ... + def get_runtime(*, args, output_dir, dashboard=None): ... def get_toolkit(*, primitives_kwargs, video_path=None): ... There is **no central list** of envs. Dropping a package under diff --git a/docs/source-en/rst_source/usage/rebot_robstride.rst b/docs/source-en/rst_source/usage/rebot_robstride.rst new file mode 100644 index 00000000..5c7366a1 --- /dev/null +++ b/docs/source-en/rst_source/usage/rebot_robstride.rst @@ -0,0 +1,119 @@ +reBot DevArm (RobStride) +======================== + +RPent can control the seven-motor reBot DevArm B601-RS directly through +``motorbridge`` and a classic SocketCAN adapter. The hardware server owns the +CAN interface exclusively and exposes guarded joint, gripper, stop, and state +operations to every RPent cerebrum. + +Install +------- + +Install the optional hardware dependencies in the active RPent environment: + +.. code-block:: bash + + uv sync --active --inexact --extra rebot-robstride + +Bring up the CAN interface before starting RPent. The server checks this state +but never invokes ``sudo`` itself: + +.. code-block:: bash + + sudo ip link set can0 up type can bitrate 1000000 + ip -details link show can0 + +Do not run another motorbridge gateway or controller against ``can0`` while the +RPent environment server is active. The server must be the sole owner of host +ID ``0xFD``. + +Passive hardware check +---------------------- + +Verify all seven motors before enabling torque: + +.. code-block:: bash + + python scripts/check_rebot_robstride.py + +The check only reads RobStride ``mechPos`` (``0x7019``) and ``mechVel`` +(``0x701A``) parameters. It never clears faults, changes modes, enables motors, +or sends a target. + +Configuration +------------- + +Copy the example and review every raw-motor limit and gain for your arm: + +.. code-block:: bash + + cp robots/rebot_robstride/config/rebot_robstride.example.yaml rebot.yaml + +The default B601-RS mapping is: + +.. list-table:: + :header-rows: 1 + + * - Motor IDs + - Model + - Role + * - 1–3 + - ``rs-06`` + - shoulder/base arm joints + * - 4–6 + - ``rs-00`` + - wrist arm joints + * - 7 + - ``rs-00`` + - gripper + +Gripper ``open_position`` and ``closed_position`` intentionally default to +null. Calibrate the installed gripper before setting them. RPent refuses every +gripper command while either endpoint is missing. + +Run +--- + +Start a physical-agent run with an explicit natural-language instruction: + +.. code-block:: bash + + python rpent/cli/main.py \ + --env rebot_robstride \ + --env-config rebot.yaml \ + --instruction "Read the current joint state and wait" \ + --cerebrum api \ + --model anthropic:claude-opus-4-8 + +The arm starts disabled. An agent must call ``get_robot_state`` and then +``enable_arm`` before any motion. ``enable_arm`` clears motor errors, selects +MIT mode, enables the controller, and immediately holds the observed pose to +avoid a startup jump. + +Available robot tools +--------------------- + +- ``get_robot_state`` — fresh positions and velocities for all motors. +- ``enable_arm`` — explicit fault-clear, mode selection, enable, and pose hold. +- ``move_joints`` — six raw-motor-radian targets through a bounded minimum-jerk + trajectory with final read-back evidence. +- ``set_gripper`` / ``open_gripper`` / ``close_gripper`` — calibrated, + normalized gripper control. +- ``stop_motion`` — torque-holding software stop; rejects later motion. +- ``reset_stop`` — clears only the software-stop latch; never enables motors. +- ``emergency_stop`` — disables all motors immediately. + +Safety limits +------------- + +``move_joints`` rejects non-finite values, targets outside the configured joint +limits, and malformed six-joint vectors. Requested durations are stretched when +needed to respect each joint's configured velocity cap. Every result includes +the target, final hardware read-back, maximum error, and ``reached`` flag. + +``emergency_stop`` removes torque; an unsupported arm may fall under gravity. +Support the arm and keep the physical emergency stop accessible during initial +hardware validation. + +The first implementation provides guarded joint-space control. It does not +claim collision avoidance, Cartesian planning, or perception-based grasping. diff --git a/pyproject.toml b/pyproject.toml index d7d91fe2..c25fa4be 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -49,6 +49,10 @@ Documentation = "https://rpent.readthedocs.io/en/latest/" rlinf = [ "rlinf @ git+https://github.com/jx-qiu/RLinf.git@feature/physicalagent", ] +rebot-robstride = [ + "motorbridge>=0.4.9; sys_platform == 'linux'", + "PyYAML>=6.0", +] [tool.uv] prerelease = "allow" diff --git a/robots/libero/__init__.py b/robots/libero/__init__.py index 49f4e630..4ac10154 100644 --- a/robots/libero/__init__.py +++ b/robots/libero/__init__.py @@ -1,14 +1,15 @@ """LIBERO environment extension.""" + from __future__ import annotations from typing import Any -from rpent.envs.prompt_bundle import PromptBundle -from rpent.envs.env_spec import EnvSpec from robots.libero.prompt_bundle import ( system_prompt, user_prompt, ) +from rpent.envs.env_spec import EnvSpec +from rpent.envs.prompt_bundle import PromptBundle def get_env_spec() -> EnvSpec: @@ -26,6 +27,13 @@ def get_env_spec() -> EnvSpec: ) +def get_runtime(*, args: Any, output_dir: str, dashboard: Any = None): + """Return the LIBERO process lifecycle adapter.""" + from robots.libero.runtime import LiberoRuntime + + return LiberoRuntime(args=args, output_dir=output_dir, dashboard=dashboard) + + def get_toolkit( *, primitives_kwargs: dict[str, Any], diff --git a/robots/libero/runtime.py b/robots/libero/runtime.py new file mode 100644 index 00000000..9592dcd9 --- /dev/null +++ b/robots/libero/runtime.py @@ -0,0 +1,171 @@ +"""LIBERO process lifecycle adapter for the generic RPent runner.""" + +from __future__ import annotations + +import os +import socket +import subprocess +import sys +import time +from pathlib import Path +from typing import Any + +from robots.libero.env_client import LiberoEnvClient +from robots.libero.toolkit import LiberoToolkit +from rpent.envs.process import start_socket_server_process, stop_socket_server_process +from rpent.envs.runtime import EnvRuntime +from rpent.utils.config import get_libero_type, get_repo_root +from rpent.utils.rpc import create_rpc_client, set_socket_endpoint +from rpent.utils.vla_client import VLAClient + + +class LiberoRuntime(EnvRuntime): + """Preserve the existing LIBERO env/VLA lifecycle behind EnvRuntime.""" + + def __init__( + self, + *, + args: Any, + output_dir: str | Path, + dashboard: Any = None, + ) -> None: + self.args = args + self.output_dir = Path(output_dir) + self.dashboard = dashboard + self._env_proc: subprocess.Popen | None = None + self._vla_proc: subprocess.Popen | None = None + + def start(self) -> LiberoToolkit: + args = self.args + if not args.suite: + raise ValueError("the LIBERO environment requires --suite") + if args.task is None: + raise ValueError("the LIBERO environment requires --task") + + vla_endpoint = args.vla_endpoint + if not args.no_driver: + env = os.environ.copy() + env["LIBERO_TYPE"] = args.libero_type or get_libero_type() + if args.cuda_device is not None: + env["CUDA_VISIBLE_DEVICES"] = str(args.cuda_device) + env.setdefault("MUJOCO_GL", "egl") + env.setdefault("ROBOT_PLATFORM", "LIBERO") + command = [ + sys.executable, + str(get_repo_root() / "robots" / "libero" / "env_server.py"), + "--suite", + args.suite, + "--task", + str(args.task), + "--seed", + str(args.seed), + "--max-episode-steps", + str(args.max_episode_steps), + "--output-dir", + str(self.output_dir), + ] + self._env_proc = start_socket_server_process( + command, + output_dir=self.output_dir, + log_name="env_server.log", + env=env, + cwd=get_repo_root(), + ) + if vla_endpoint is None: + vla_endpoint, self._vla_proc = _start_vla_server( + cuda_device=args.cuda_device, + log_path=self.output_dir / "vla_server.log", + ) + else: + if args.env_port <= 0: + raise ValueError( + "--no-driver requires --env-port pointing at an existing env_server" + ) + if vla_endpoint is None: + raise ValueError( + "--no-driver requires --vla-endpoint pointing at an existing vla_server" + ) + set_socket_endpoint(self.output_dir, args.env_endpoint, args.env_port) + + expected_meta = { + "suite": args.suite, + "task": args.task, + "seed": args.seed, + "max_episode_steps": args.max_episode_steps, + } + env_client = LiberoEnvClient( + create_rpc_client(self.output_dir), expected_meta=expected_meta + ) + return LiberoToolkit( + primitives_kwargs={ + "env": env_client, + "model": VLAClient(vla_endpoint), + }, + video_path=str(self.output_dir / "episode.mp4"), + dashboard=self.dashboard, + ) + + def stop(self) -> None: + stop_socket_server_process( + self._env_proc, + output_dir=self.output_dir, + ) + _stop_vla_server(self._vla_proc) + self._env_proc = None + self._vla_proc = None + + +def _start_vla_server( + *, + host: str = "127.0.0.1", + port: int = 0, + cuda_device: str | None = None, + log_path: str | Path | None = None, +) -> tuple[str, subprocess.Popen]: + if port == 0: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as sock: + sock.bind((host, 0)) + port = int(sock.getsockname()[1]) + + env = os.environ.copy() + if cuda_device is not None: + env["CUDA_VISIBLE_DEVICES"] = str(cuda_device) + command = [ + sys.executable, + str(get_repo_root() / "robots" / "libero" / "vla_server.py"), + "--host", + host, + "--port", + str(port), + ] + log_file = Path(log_path).open("a", encoding="utf-8") if log_path else None + proc = subprocess.Popen( + command, + stdout=log_file, + stderr=subprocess.STDOUT if log_file else None, + env=env, + ) + base_url = f"http://{host}:{port}" + client = VLAClient(base_url) + deadline = time.monotonic() + 300.0 + while time.monotonic() < deadline: + if proc.poll() is not None: + raise RuntimeError("vla server exited before becoming ready") + try: + if client.healthz(): + return base_url, proc + except Exception: + pass + time.sleep(2.0) + proc.terminate() + raise RuntimeError("vla server not ready after 300s") + + +def _stop_vla_server(proc: subprocess.Popen | None, timeout_s: float = 10.0) -> None: + if proc is None or proc.poll() is not None: + return + proc.terminate() + try: + proc.wait(timeout=timeout_s) + except subprocess.TimeoutExpired: + proc.kill() diff --git a/robots/rebot_robstride/__init__.py b/robots/rebot_robstride/__init__.py new file mode 100644 index 00000000..60fa5749 --- /dev/null +++ b/robots/rebot_robstride/__init__.py @@ -0,0 +1,38 @@ +"""reBot DevArm RobStride environment extension.""" + +from __future__ import annotations + +from typing import Any + +from robots.rebot_robstride.prompt_bundle import system_prompt, user_prompt +from rpent.envs.env_spec import EnvSpec +from rpent.envs.prompt_bundle import PromptBundle + + +def get_env_spec() -> EnvSpec: + """Return the physical reBot environment identity and prompts.""" + return EnvSpec( + name="rebot_robstride", + prompts=PromptBundle(system=system_prompt, user=user_prompt), + ) + + +def get_runtime(*, args: Any, output_dir: str, dashboard: Any = None): + """Return the reBot RobStride process lifecycle adapter.""" + from robots.rebot_robstride.runtime import RebotRobstrideRuntime + + return RebotRobstrideRuntime( + args=args, + output_dir=output_dir, + dashboard=dashboard, + ) + + +def get_toolkit(*, primitives_kwargs: dict[str, Any], dashboard: Any = None, **_): + """Build the reBot toolkit for callers that manage transport themselves.""" + from robots.rebot_robstride.toolkit import RebotRobstrideToolkit + + return RebotRobstrideToolkit( + env=primitives_kwargs["env"], + dashboard=dashboard, + ) diff --git a/robots/rebot_robstride/config.py b/robots/rebot_robstride/config.py new file mode 100644 index 00000000..2a2fab4a --- /dev/null +++ b/robots/rebot_robstride/config.py @@ -0,0 +1,195 @@ +"""Validated configuration for the reBot DevArm RobStride backend.""" + +from __future__ import annotations + +import math +from dataclasses import dataclass, replace +from pathlib import Path +from typing import Any + + +@dataclass(frozen=True) +class JointConfig: + """One RobStride arm joint in raw motor coordinates.""" + + name: str + motor_id: int + model: str + lower: float + upper: float + kp: float + kd: float + max_velocity: float = 0.5 + + +@dataclass(frozen=True) +class GripperConfig: + """RobStride gripper motor and optional calibrated travel endpoints.""" + + motor_id: int = 7 + model: str = "rs-00" + kp: float = 20.0 + kd: float = 1.0 + max_velocity: float = 1.0 + open_position: float | None = None + closed_position: float | None = None + + +@dataclass(frozen=True) +class RebotConfig: + """Complete hardware and safety configuration.""" + + channel: str + bitrate: int + control_rate_hz: float + read_timeout_ms: int + settle_tolerance: float + settle_timeout_s: float + shutdown_policy: str + joints: tuple[JointConfig, ...] + gripper: GripperConfig + + +def default_config() -> RebotConfig: + """Return conservative defaults for the seven-motor B601-RS build.""" + joints = ( + JointConfig("joint1", 1, "rs-06", -2.8, 2.8, 50.0, 3.0), + JointConfig("joint2", 2, "rs-06", -3.14, 0.0, 150.0, 10.0), + JointConfig("joint3", 3, "rs-06", -3.14, 0.0, 150.0, 10.0), + JointConfig("joint4", 4, "rs-00", -1.57, 1.57, 50.0, 5.0), + JointConfig("joint5", 5, "rs-00", -1.57, 1.57, 50.0, 4.0), + JointConfig("joint6", 6, "rs-00", -3.14, 3.14, 50.0, 4.0), + ) + return _validate( + RebotConfig( + channel="can0", + bitrate=1_000_000, + control_rate_hz=50.0, + read_timeout_ms=100, + settle_tolerance=0.03, + settle_timeout_s=2.0, + shutdown_policy="disable", + joints=joints, + gripper=GripperConfig(), + ) + ) + + +def load_config(path: str | Path | None = None) -> RebotConfig: + """Load a YAML override on top of :func:`default_config`.""" + config = default_config() + if path is None: + return config + + try: + import yaml + except ImportError as exc: # pragma: no cover - installation error path + raise RuntimeError( + "PyYAML is required to load a reBot config; install rpent[rebot-robstride]" + ) from exc + + raw = yaml.safe_load(Path(path).read_text(encoding="utf-8")) or {} + if not isinstance(raw, dict): + raise ValueError("reBot config must contain a YAML mapping") + + scalar_fields = { + "channel", + "bitrate", + "control_rate_hz", + "read_timeout_ms", + "settle_tolerance", + "settle_timeout_s", + "shutdown_policy", + } + updates = {key: raw[key] for key in scalar_fields if key in raw} + + joints = config.joints + if "joints" in raw: + joint_rows = raw["joints"] + if not isinstance(joint_rows, list) or len(joint_rows) != 6: + raise ValueError("joints must be a list containing exactly six entries") + joints = tuple(_joint_from_mapping(row) for row in joint_rows) + + gripper = config.gripper + if "gripper" in raw: + row = raw["gripper"] or {} + if not isinstance(row, dict): + raise ValueError("gripper must be a mapping") + allowed = {field.name for field in GripperConfig.__dataclass_fields__.values()} + unknown = sorted(set(row) - allowed) + if unknown: + raise ValueError(f"unknown gripper fields: {', '.join(unknown)}") + gripper = replace(gripper, **row) + + return _validate(replace(config, joints=joints, gripper=gripper, **updates)) + + +def _joint_from_mapping(row: Any) -> JointConfig: + if not isinstance(row, dict): + raise ValueError("each joint entry must be a mapping") + required = {"name", "motor_id", "model", "lower", "upper", "kp", "kd"} + missing = sorted(required - set(row)) + if missing: + raise ValueError(f"joint entry missing fields: {', '.join(missing)}") + allowed = required | {"max_velocity"} + unknown = sorted(set(row) - allowed) + if unknown: + raise ValueError(f"unknown joint fields: {', '.join(unknown)}") + return JointConfig(**row) + + +def _validate(config: RebotConfig) -> RebotConfig: + if not config.channel: + raise ValueError("channel must be non-empty") + if config.bitrate <= 0: + raise ValueError("bitrate must be positive") + + if config.control_rate_hz <= 0: + raise ValueError("control_rate_hz must be positive") + if config.read_timeout_ms <= 0: + raise ValueError("read_timeout_ms must be positive") + if config.settle_tolerance <= 0 or config.settle_timeout_s <= 0: + raise ValueError("settle tolerances and timeouts must be positive") + if config.shutdown_policy not in {"disable", "hold"}: + raise ValueError("shutdown_policy must be 'disable' or 'hold'") + if len(config.joints) != 6: + raise ValueError("exactly six arm joints are required") + + motor_ids = [joint.motor_id for joint in config.joints] + [config.gripper.motor_id] + if len(set(motor_ids)) != len(motor_ids): + raise ValueError("motor IDs must be unique") + if any(not 1 <= motor_id <= 0xFF for motor_id in motor_ids): + raise ValueError("motor IDs must be in 1..255") + + names = [joint.name for joint in config.joints] + if len(set(names)) != len(names): + raise ValueError("joint names must be unique") + + for joint in config.joints: + values = (joint.lower, joint.upper, joint.kp, joint.kd, joint.max_velocity) + if not all(math.isfinite(value) for value in values): + raise ValueError(f"{joint.name} contains a non-finite value") + if joint.lower >= joint.upper: + raise ValueError(f"{joint.name} lower limit must be below upper limit") + if joint.kp < 0 or joint.kd < 0: + raise ValueError(f"{joint.name} gains must be non-negative") + if not 0 < joint.max_velocity <= 1.0: + raise ValueError(f"{joint.name} max_velocity must be in (0, 1.0]") + + gripper = config.gripper + if (gripper.open_position is None) != (gripper.closed_position is None): + raise ValueError( + "gripper open_position and closed_position must both be set or both be null" + ) + gripper_values = (gripper.kp, gripper.kd, gripper.max_velocity) + if not all(math.isfinite(value) for value in gripper_values): + raise ValueError("gripper contains a non-finite value") + if gripper.kp < 0 or gripper.kd < 0 or gripper.max_velocity <= 0: + raise ValueError("gripper gains must be non-negative and max_velocity positive") + if gripper.open_position is not None: + endpoints = (gripper.open_position, gripper.closed_position) + if not all(value is not None and math.isfinite(value) for value in endpoints): + raise ValueError("gripper endpoints must be finite") + if gripper.open_position == gripper.closed_position: + raise ValueError("gripper endpoints must differ") + return config diff --git a/robots/rebot_robstride/config/rebot_robstride.example.yaml b/robots/rebot_robstride/config/rebot_robstride.example.yaml new file mode 100644 index 00000000..43511d26 --- /dev/null +++ b/robots/rebot_robstride/config/rebot_robstride.example.yaml @@ -0,0 +1,29 @@ +# reBot DevArm B601-RS: RobStride motors over classic SocketCAN. +# Bring the interface up before starting RPent: +# sudo ip link set can0 up type can bitrate 1000000 +channel: can0 +bitrate: 1000000 +control_rate_hz: 50 +read_timeout_ms: 100 +settle_tolerance: 0.03 +settle_timeout_s: 2.0 +shutdown_policy: disable + +# Limits are raw motor coordinates. Keep max_velocity <= 1.0 rad/s. +joints: + - {name: joint1, motor_id: 1, model: rs-06, lower: -2.8, upper: 2.8, kp: 50, kd: 3, max_velocity: 0.5} + - {name: joint2, motor_id: 2, model: rs-06, lower: -3.14, upper: 0.0, kp: 150, kd: 10, max_velocity: 0.5} + - {name: joint3, motor_id: 3, model: rs-06, lower: -3.14, upper: 0.0, kp: 150, kd: 10, max_velocity: 0.5} + - {name: joint4, motor_id: 4, model: rs-00, lower: -1.57, upper: 1.57, kp: 50, kd: 5, max_velocity: 0.5} + - {name: joint5, motor_id: 5, model: rs-00, lower: -1.57, upper: 1.57, kp: 50, kd: 4, max_velocity: 0.5} + - {name: joint6, motor_id: 6, model: rs-00, lower: -3.14, upper: 3.14, kp: 50, kd: 4, max_velocity: 0.5} + +gripper: + motor_id: 7 + model: rs-00 + kp: 20 + kd: 1 + max_velocity: 1.0 + # Deliberately unset: calibrate this specific gripper before commanding it. + open_position: + closed_position: diff --git a/robots/rebot_robstride/driver.py b/robots/rebot_robstride/driver.py new file mode 100644 index 00000000..7b82b1b2 --- /dev/null +++ b/robots/rebot_robstride/driver.py @@ -0,0 +1,363 @@ +"""Safe motorbridge driver for the seven-motor reBot DevArm RobStride arm.""" + +from __future__ import annotations + +import math +import threading +import time +from collections.abc import Callable, Sequence +from typing import Any + +from robots.rebot_robstride.config import JointConfig, RebotConfig + +MECH_POS = 0x7019 +MECH_VEL = 0x701A +ROBSTRIDE_HOST_ID = 0xFD +MIT_MODE = 1 + + +def _default_controller_factory(channel: str): + try: + from motorbridge import Controller + except ImportError as exc: # pragma: no cover - installation error path + raise RuntimeError( + "motorbridge is required; install the rpent[rebot-robstride] extra" + ) from exc + return Controller(channel) + + +def _min_jerk(value: float) -> float: + return 10 * value**3 - 15 * value**4 + 6 * value**5 + + +class RebotRobstrideDriver: + """Own one RobStride CAN session and fail closed around every command.""" + + def __init__( + self, + config: RebotConfig, + *, + controller_factory: Callable[[str], Any] | None = None, + clock: Callable[[], float] = time.monotonic, + sleep: Callable[[float], None] = time.sleep, + ) -> None: + self.config = config + self._controller_factory = controller_factory or _default_controller_factory + self._clock = clock + self._sleep = sleep + self._lock = threading.RLock() + self._controller = None + self._motors: dict[int, Any] = {} + self._enabled = False + self._stopped = False + self._last_targets: list[float] | None = None + + @property + def connected(self) -> bool: + return self._controller is not None + + @property + def enabled(self) -> bool: + return self._enabled + + @property + def stopped(self) -> bool: + return self._stopped + + def connect(self) -> dict[str, Any]: + """Open CAN, register all motors, and take a passive state snapshot.""" + with self._lock: + if self.connected: + return self.state() + controller = self._controller_factory(self.config.channel) + try: + motor_configs = [*self.config.joints, self.config.gripper] + motors = { + motor.motor_id: controller.add_robstride_motor( + motor_id=motor.motor_id, + feedback_id=ROBSTRIDE_HOST_ID, + model=motor.model, + ) + for motor in motor_configs + } + self._controller = controller + self._motors = motors + snapshot = self.state() + self._last_targets = list(snapshot["joint_positions"]) + return snapshot + except Exception: + try: + controller.close() + finally: + self._controller = None + self._motors = {} + raise + + def state(self) -> dict[str, Any]: + """Return a fresh all-motor snapshot using RobStride parameter reads.""" + with self._lock: + self._require_connected() + positions: list[float] = [] + velocities: list[float] = [] + for joint in self.config.joints: + position, velocity = self._read_motor(joint.motor_id) + positions.append(position) + velocities.append(velocity) + gripper_position, gripper_velocity = self._read_motor( + self.config.gripper.motor_id + ) + return { + "connected": True, + "enabled": self._enabled, + "stopped": self._stopped, + "channel": self.config.channel, + "joint_names": [joint.name for joint in self.config.joints], + "joint_positions": positions, + "joint_velocities": velocities, + "gripper_position": gripper_position, + "gripper_velocity": gripper_velocity, + "timestamp": self._clock(), + } + + def enable(self) -> dict[str, Any]: + """Clear faults, select MIT mode, and hold the observed pose.""" + with self._lock: + self._require_connected() + controller = self._controller + assert controller is not None + snapshot = self.state() + hold_targets = [ + *snapshot["joint_positions"], + snapshot["gripper_position"], + ] + motor_configs = [*self.config.joints, self.config.gripper] + for motor_config in motor_configs: + motor = self._motors[motor_config.motor_id] + motor.clear_error() + motor.ensure_mode(MIT_MODE, timeout_ms=1000) + try: + controller.enable_all() + self._enabled = True + for motor_config, target in zip(motor_configs, hold_targets): + self._send_mit(motor_config, target) + except Exception: + self._stopped = True + try: + controller.disable_all() + finally: + self._enabled = False + raise + self._stopped = False + self._last_targets = list(snapshot["joint_positions"]) + return { + "enabled": True, + "hold_positions": list(snapshot["joint_positions"]), + "gripper_hold_position": snapshot["gripper_position"], + } + + def move_joints( + self, + positions: Sequence[float], + *, + duration_s: float = 2.0, + ) -> dict[str, Any]: + """Execute a minimum-jerk joint trajectory and verify final feedback.""" + with self._lock: + self._require_motion_ready() + target = [float(value) for value in positions] + if len(target) != len(self.config.joints): + raise ValueError("positions must contain exactly six joint values") + if not all(math.isfinite(value) for value in target): + raise ValueError("joint targets must be finite") + self._validate_joint_limits(target) + if not math.isfinite(duration_s) or duration_s <= 0: + raise ValueError("duration_s must be positive and finite") + + start_state = self.state() + start = list(start_state["joint_positions"]) + minimum_duration = max( + abs(goal - initial) / joint.max_velocity + for initial, goal, joint in zip(start, target, self.config.joints) + ) + actual_duration = max(float(duration_s), minimum_duration) + steps = max(2, math.ceil(actual_duration * self.config.control_rate_hz)) + interval = actual_duration / steps + start_time = self._clock() + try: + for index in range(1, steps + 1): + scale = _min_jerk(index / steps) + waypoint = [ + initial + (goal - initial) * scale + for initial, goal in zip(start, target) + ] + for joint, position in zip(self.config.joints, waypoint): + self._send_mit(joint, position) + self._last_targets = waypoint + remaining = start_time + index * interval - self._clock() + if remaining > 0: + self._sleep(remaining) + + final = self._wait_for_target(target) + except Exception: + self._stopped = True + raise + errors = [abs(observed - goal) for observed, goal in zip(final, target)] + max_error = max(errors) + return { + "target_positions": target, + "final_positions": final, + "max_error": max_error, + "reached": max_error <= self.config.settle_tolerance, + "requested_duration_s": float(duration_s), + "actual_duration_s": actual_duration, + } + + def set_gripper( + self, + position: float, + *, + duration_s: float = 1.0, + ) -> dict[str, Any]: + """Move the calibrated gripper; 0.0=open and 1.0=closed.""" + with self._lock: + self._require_motion_ready() + normalized = float(position) + if not math.isfinite(normalized) or not 0.0 <= normalized <= 1.0: + raise ValueError("gripper position must be in [0.0, 1.0]") + if not math.isfinite(duration_s) or duration_s <= 0: + raise ValueError("duration_s must be positive and finite") + gripper = self.config.gripper + if gripper.open_position is None or gripper.closed_position is None: + raise RuntimeError( + "gripper is not calibrated; configure open_position and closed_position" + ) + target = gripper.open_position + normalized * ( + gripper.closed_position - gripper.open_position + ) + start, _ = self._read_motor(gripper.motor_id) + minimum_duration = abs(target - start) / gripper.max_velocity + actual_duration = max(float(duration_s), minimum_duration) + steps = max(2, math.ceil(actual_duration * self.config.control_rate_hz)) + interval = actual_duration / steps + start_time = self._clock() + try: + for index in range(1, steps + 1): + scale = _min_jerk(index / steps) + waypoint = start + (target - start) * scale + self._send_mit(gripper, waypoint) + remaining = start_time + index * interval - self._clock() + if remaining > 0: + self._sleep(remaining) + final, _ = self._read_motor(gripper.motor_id) + except Exception: + self._stopped = True + raise + return { + "normalized_position": normalized, + "target_position": target, + "final_position": final, + "actual_duration_s": actual_duration, + } + + def stop_motion(self) -> dict[str, Any]: + """Latch a torque-holding software stop and reject new trajectories.""" + with self._lock: + self._require_connected() + self._stopped = True + if self._enabled: + snapshot = self.state() + for joint, position in zip( + self.config.joints, snapshot["joint_positions"] + ): + self._send_mit(joint, position) + self._last_targets = list(snapshot["joint_positions"]) + return {"enabled": self._enabled, "stopped": True} + + def reset_stop(self) -> dict[str, Any]: + """Clear only the software-stop latch; it never enables motors.""" + with self._lock: + self._require_connected() + self._stopped = False + return {"enabled": self._enabled, "stopped": False} + + def emergency_stop(self) -> dict[str, Any]: + """Disable every motor immediately; the unsupported arm may fall.""" + with self._lock: + self._require_connected() + self._controller.disable_all() + self._enabled = False + self._stopped = True + return {"enabled": False, "stopped": True} + + def close(self) -> None: + """Apply the configured shutdown policy and release the CAN controller.""" + with self._lock: + if self._controller is None: + return + try: + if self.config.shutdown_policy == "disable" and self._enabled: + self._controller.disable_all() + self._enabled = False + elif self._enabled and self._last_targets is not None: + for joint, target in zip(self.config.joints, self._last_targets): + self._send_mit(joint, target) + finally: + self._controller.close() + self._controller = None + self._motors = {} + self._enabled = False + self._stopped = True + + def _read_motor(self, motor_id: int) -> tuple[float, float]: + motor = self._motors[motor_id] + position = float( + motor.robstride_get_param_f32( + MECH_POS, timeout_ms=self.config.read_timeout_ms + ) + ) + velocity = float( + motor.robstride_get_param_f32( + MECH_VEL, timeout_ms=self.config.read_timeout_ms + ) + ) + if not math.isfinite(position) or not math.isfinite(velocity): + raise RuntimeError(f"motor {motor_id} returned non-finite feedback") + return position, velocity + + def _send_mit(self, config: JointConfig | Any, position: float) -> None: + self._motors[config.motor_id].send_mit( + float(position), 0.0, float(config.kp), float(config.kd), 0.0 + ) + + def _wait_for_target(self, target: list[float]) -> list[float]: + deadline = self._clock() + self.config.settle_timeout_s + final: list[float] = [] + while True: + final = list(self.state()["joint_positions"]) + if ( + max(abs(value - goal) for value, goal in zip(final, target)) + <= self.config.settle_tolerance + ): + return final + if self._clock() >= deadline: + return final + self._sleep(0.05) + + def _validate_joint_limits(self, target: list[float]) -> None: + for joint, value in zip(self.config.joints, target): + if not joint.lower <= value <= joint.upper: + raise ValueError( + f"{joint.name} target {value:.6f} outside " + f"[{joint.lower:.6f}, {joint.upper:.6f}]" + ) + + def _require_connected(self) -> None: + if self._controller is None: + raise RuntimeError("arm is not connected") + + def _require_motion_ready(self) -> None: + self._require_connected() + if not self._enabled: + raise RuntimeError("arm is not enabled") + if self._stopped: + raise RuntimeError("arm is stopped; call reset_stop before moving") diff --git a/robots/rebot_robstride/env_client.py b/robots/rebot_robstride/env_client.py new file mode 100644 index 00000000..a03b4975 --- /dev/null +++ b/robots/rebot_robstride/env_client.py @@ -0,0 +1,50 @@ +"""Agent-side RPC client for the reBot DevArm RobStride server.""" + +from __future__ import annotations + +from typing import Any + +from rpent.utils.rpc import RpcClient + +_DEFAULT_TIMEOUT_S = 10.0 +_MOTION_TIMEOUT_S = 120.0 + + +class RebotRobstrideEnvClient: + """Typed facade over the stable ``robot.*`` RPC method names.""" + + def __init__(self, client: RpcClient) -> None: + self._client = client + + def state(self) -> dict[str, Any]: + return self._client.call("robot.state", timeout_s=_DEFAULT_TIMEOUT_S) + + def enable(self) -> dict[str, Any]: + return self._client.call("robot.enable", timeout_s=_DEFAULT_TIMEOUT_S) + + def move_joints( + self, positions: list[float], *, duration_s: float = 2.0 + ) -> dict[str, Any]: + return self._client.call( + "robot.move_joints", + kwargs={"positions": positions, "duration_s": duration_s}, + timeout_s=_MOTION_TIMEOUT_S, + ) + + def set_gripper( + self, position: float, *, duration_s: float = 1.0 + ) -> dict[str, Any]: + return self._client.call( + "robot.set_gripper", + kwargs={"position": position, "duration_s": duration_s}, + timeout_s=_MOTION_TIMEOUT_S, + ) + + def stop_motion(self) -> dict[str, Any]: + return self._client.call("robot.stop_motion", timeout_s=_DEFAULT_TIMEOUT_S) + + def reset_stop(self) -> dict[str, Any]: + return self._client.call("robot.reset_stop", timeout_s=_DEFAULT_TIMEOUT_S) + + def emergency_stop(self) -> dict[str, Any]: + return self._client.call("robot.emergency_stop", timeout_s=_DEFAULT_TIMEOUT_S) diff --git a/robots/rebot_robstride/env_server.py b/robots/rebot_robstride/env_server.py new file mode 100644 index 00000000..0bccefc1 --- /dev/null +++ b/robots/rebot_robstride/env_server.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python3 +"""Single-owner SocketCAN RPC server for the reBot DevArm RobStride arm.""" + +from __future__ import annotations + +import argparse +import json +import re +import signal +import subprocess +import threading +from pathlib import Path +from typing import Any + +from robots.rebot_robstride.config import load_config +from robots.rebot_robstride.driver import RebotRobstrideDriver +from rpent.utils.socket_rpc import SocketRpcServer + + +def make_dispatch(driver: Any, shutdown_event: threading.Event): + """Return the strict RPC dispatcher used by the hardware server.""" + handlers = { + "robot.state": driver.state, + "robot.enable": driver.enable, + "robot.move_joints": driver.move_joints, + "robot.set_gripper": driver.set_gripper, + "robot.stop_motion": driver.stop_motion, + "robot.reset_stop": driver.reset_stop, + "robot.emergency_stop": driver.emergency_stop, + } + + def dispatch(method: str, args: tuple, kwargs: dict): + if method == "shutdown": + shutdown_event.set() + return {"ok": True} + handler = handlers.get(method) + if handler is None: + raise ValueError(f"unknown RPC method: {method!r}") + return handler(*args, **kwargs) + + return dispatch + + +def validate_socketcan(channel: str, bitrate: int) -> None: + """Fail with an actionable message unless SocketCAN is up at the expected rate.""" + result = subprocess.run( + ["ip", "-details", "link", "show", channel], + check=False, + capture_output=True, + text=True, + ) + if result.returncode != 0: + raise RuntimeError( + f"SocketCAN interface {channel!r} does not exist; connect a CAN adapter first" + ) + output = result.stdout + first_line = output.splitlines()[0] if output.splitlines() else "" + flags = first_line.partition("<")[2].partition(">")[0].split(",") + if "UP" not in flags: + raise RuntimeError( + f"SocketCAN interface {channel!r} is down; run: " + f"sudo ip link set {channel} up type can bitrate {bitrate}" + ) + match = re.search(r"\bbitrate\s+(\d+)", output) + if match is None or int(match.group(1)) != bitrate: + actual = match.group(1) if match else "unknown" + raise RuntimeError( + f"SocketCAN interface {channel!r} bitrate is {actual}, expected {bitrate}; " + f"reconfigure it before starting RPent" + ) + + +def _build_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--host", default="127.0.0.1") + parser.add_argument("--port", type=int, default=0) + parser.add_argument("--config", type=Path, default=None) + return parser + + +def main() -> int: + args = _build_parser().parse_args() + config = load_config(args.config) + validate_socketcan(config.channel, config.bitrate) + + driver = RebotRobstrideDriver(config) + shutdown_event = threading.Event() + server = None + try: + initial_state = driver.connect() + server = SocketRpcServer( + (args.host, args.port), make_dispatch(driver, shutdown_event) + ) + host, port = server.server_address + threading.Thread(target=server.serve_forever, daemon=True).start() + + def request_shutdown(_signum, _frame) -> None: + shutdown_event.set() + + signal.signal(signal.SIGINT, request_shutdown) + signal.signal(signal.SIGTERM, request_shutdown) + print( + json.dumps( + { + "event": "transport_ready", + "kind": "socket", + "host": host, + "port": port, + "environment": "rebot_robstride", + "motors_seen": len(initial_state["joint_positions"]) + 1, + } + ), + flush=True, + ) + shutdown_event.wait() + return 0 + finally: + if server is not None: + server.shutdown() + server.server_close() + driver.close() + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/robots/rebot_robstride/prompt_bundle.py b/robots/rebot_robstride/prompt_bundle.py new file mode 100644 index 00000000..5b6795fc --- /dev/null +++ b/robots/rebot_robstride/prompt_bundle.py @@ -0,0 +1,13 @@ +"""Prompt bundle assembly for the reBot DevArm RobStride environment.""" + +from __future__ import annotations + +from robots.rebot_robstride.prompts.system import SYSTEM, USER + + +def system_prompt() -> str: + return SYSTEM + + +def user_prompt() -> dict: + return dict(USER) diff --git a/robots/rebot_robstride/prompts/__init__.py b/robots/rebot_robstride/prompts/__init__.py new file mode 100644 index 00000000..032abf5f --- /dev/null +++ b/robots/rebot_robstride/prompts/__init__.py @@ -0,0 +1 @@ +"""Prompt fragments for the reBot DevArm RobStride environment.""" diff --git a/robots/rebot_robstride/prompts/system.py b/robots/rebot_robstride/prompts/system.py new file mode 100644 index 00000000..0ca797e8 --- /dev/null +++ b/robots/rebot_robstride/prompts/system.py @@ -0,0 +1,26 @@ +"""Prompt fragments for guarded physical reBot control.""" + +from __future__ import annotations + +SYSTEM = """ +You control a physical reBot DevArm with RobStride motors through guarded RPent tools. + +Safety rules: +- Always call get_robot_state before enable_arm and before planning motion. +- Motors start disabled. Never claim motion happened until a tool returns reached=true + with final_positions read back from the hardware. +- Use small, deliberate joint-space moves. Respect the configured raw-motor joint limits. +- Never retry a rejected or failed motion blindly; read state and explain the failure. +- stop_motion holds the observed pose and latches a software stop. +- emergency_stop disables torque. An unsupported arm can fall under gravity afterward. +- Gripper tools refuse motion until open and closed endpoints are calibrated. +- Call finish when the instruction is complete, failed, or unsafe to continue. +""" + +USER = { + "Task": """ + - instruction: {{instruction}} + - environment: rebot_robstride + - output_dir: {{output_dir}} + """, +} diff --git a/robots/rebot_robstride/runtime.py b/robots/rebot_robstride/runtime.py new file mode 100644 index 00000000..1bd87ef3 --- /dev/null +++ b/robots/rebot_robstride/runtime.py @@ -0,0 +1,67 @@ +"""Process lifecycle adapter for the reBot DevArm RobStride environment.""" + +from __future__ import annotations + +import subprocess +import sys +from pathlib import Path +from typing import Any + +from robots.rebot_robstride.env_client import RebotRobstrideEnvClient +from robots.rebot_robstride.toolkit import RebotRobstrideToolkit +from rpent.envs.process import start_socket_server_process, stop_socket_server_process +from rpent.envs.runtime import EnvRuntime +from rpent.utils.config import get_repo_root +from rpent.utils.rpc import create_rpc_client, set_socket_endpoint + + +class RebotRobstrideRuntime(EnvRuntime): + """Start or attach to the single-owner RobStride hardware server.""" + + def __init__( + self, + *, + args: Any, + output_dir: str | Path, + dashboard: Any = None, + ) -> None: + self.args = args + self.output_dir = Path(output_dir) + self.dashboard = dashboard + self._process: subprocess.Popen | None = None + + def start(self) -> RebotRobstrideToolkit: + if not self.args.no_driver: + command = [ + sys.executable, + str(get_repo_root() / "robots" / "rebot_robstride" / "env_server.py"), + ] + env_config = getattr(self.args, "env_config", None) + if env_config: + command.extend(["--config", str(env_config)]) + self._process = start_socket_server_process( + command, + output_dir=self.output_dir, + log_name="env_server.log", + cwd=get_repo_root(), + ready_timeout_s=30.0, + ) + else: + if self.args.env_port <= 0: + raise ValueError( + "--no-driver requires --env-port pointing at an existing " + "reBot RobStride server" + ) + set_socket_endpoint( + self.output_dir, self.args.env_endpoint, self.args.env_port + ) + + client = RebotRobstrideEnvClient(create_rpc_client(self.output_dir)) + return RebotRobstrideToolkit(env=client, dashboard=self.dashboard) + + def stop(self) -> None: + stop_socket_server_process( + self._process, + output_dir=self.output_dir, + ) + self._process = None diff --git a/robots/rebot_robstride/toolkit.py b/robots/rebot_robstride/toolkit.py new file mode 100644 index 00000000..3faefac7 --- /dev/null +++ b/robots/rebot_robstride/toolkit.py @@ -0,0 +1,134 @@ +"""RPent tool surface for the physical reBot DevArm RobStride arm.""" + +from __future__ import annotations + +from typing import Any + +from rpent.tools.toolkit import Toolkit + +TOOLS_SPEC = [ + { + "name": "get_robot_state", + "description": ( + "Read fresh RobStride joint, velocity, gripper, enable, and stop state. " + "Call this before enabling or planning any motion." + ), + "input_schema": {"type": "object", "properties": {}, "required": []}, + }, + { + "name": "enable_arm", + "description": ( + "Explicitly enable the arm after reading state. The driver clears faults, " + "selects MIT mode, and holds the observed pose before returning." + ), + "input_schema": {"type": "object", "properties": {}, "required": []}, + }, + { + "name": "move_joints", + "description": ( + "Move six arm joints in raw motor radians through a bounded minimum-jerk " + "trajectory. Returns final read-back evidence." + ), + "input_schema": { + "type": "object", + "properties": { + "positions": { + "type": "array", + "items": {"type": "number"}, + "minItems": 6, + "maxItems": 6, + }, + "duration_s": { + "type": "number", + "minimum": 0.1, + "default": 2.0, + }, + }, + "required": ["positions"], + }, + }, + { + "name": "set_gripper", + "description": ( + "Move the calibrated gripper to a normalized position: 0.0=open, " + "1.0=closed. Refuses to move when endpoints are not configured." + ), + "input_schema": { + "type": "object", + "properties": { + "position": {"type": "number", "minimum": 0.0, "maximum": 1.0}, + "duration_s": {"type": "number", "minimum": 0.1, "default": 1.0}, + }, + "required": ["position"], + }, + }, + { + "name": "open_gripper", + "description": "Open the calibrated gripper with a bounded trajectory.", + "input_schema": { + "type": "object", + "properties": { + "duration_s": {"type": "number", "minimum": 0.1, "default": 1.0} + }, + "required": [], + }, + }, + { + "name": "close_gripper", + "description": "Close the calibrated gripper with a bounded trajectory.", + "input_schema": { + "type": "object", + "properties": { + "duration_s": {"type": "number", "minimum": 0.1, "default": 1.0} + }, + "required": [], + }, + }, + { + "name": "stop_motion", + "description": ( + "Latch a software stop, hold the current observed pose, and reject new " + "motions until reset_stop is called." + ), + "input_schema": {"type": "object", "properties": {}, "required": []}, + }, + { + "name": "reset_stop", + "description": "Clear the software-stop latch. This never enables motors.", + "input_schema": {"type": "object", "properties": {}, "required": []}, + }, + { + "name": "emergency_stop", + "description": ( + "Immediately disable every motor. WARNING: an unsupported arm may fall " + "under gravity after this torque-off operation." + ), + "input_schema": {"type": "object", "properties": {}, "required": []}, + }, +] + + +class RebotRobstrideToolkit(Toolkit): + """Common RPent tools plus guarded physical-arm operations.""" + + def __init__(self, *, env: Any, dashboard: Any = None) -> None: + super().__init__(dashboard=dashboard) + self._env = env + specs = {spec["name"]: spec for spec in TOOLS_SPEC} + self.add_tool("get_robot_state", specs["get_robot_state"], env.state) + self.add_tool("enable_arm", specs["enable_arm"], env.enable) + self.add_tool("move_joints", specs["move_joints"], env.move_joints) + self.add_tool("set_gripper", specs["set_gripper"], env.set_gripper) + self.add_tool( + "open_gripper", + specs["open_gripper"], + lambda duration_s=1.0: env.set_gripper(0.0, duration_s=duration_s), + ) + self.add_tool( + "close_gripper", + specs["close_gripper"], + lambda duration_s=1.0: env.set_gripper(1.0, duration_s=duration_s), + ) + self.add_tool("stop_motion", specs["stop_motion"], env.stop_motion) + self.add_tool("reset_stop", specs["reset_stop"], env.reset_stop) + self.add_tool("emergency_stop", specs["emergency_stop"], env.emergency_stop) diff --git a/rpent/cli/main.py b/rpent/cli/main.py index ff5bff26..9c6a9cc2 100644 --- a/rpent/cli/main.py +++ b/rpent/cli/main.py @@ -1,4 +1,5 @@ """Physical agent main CLI entrypoint.""" + # `rpent/cli/` # # CLI entrypoints for RPent (currently just `main.py`). @@ -28,225 +29,23 @@ import argparse import json -import os -import queue import shlex -import subprocess import sys import threading import time from datetime import datetime from pathlib import Path +from rpent.cerebrum.base import build_cerebrum # noqa: E402 +from rpent.envs import get_env_spec, get_runtime # noqa: E402 from rpent.utils.config import ( - get_libero_type, get_repo_root, ) - -from rpent.cerebrum.base import build_cerebrum # noqa: E402 -from rpent.envs import get_env_spec, get_toolkit # noqa: E402 -from rpent.utils.rpc import ( # noqa: E402 - create_rpc_client, - set_socket_endpoint, -) -from rpent.utils.vla_client import VLAClient # noqa: E402 -from robots.libero.env_client import LiberoEnvClient # noqa: E402 from rpent.utils.logging import get_logger, init_output_dir # noqa: E402 logger = get_logger("agent") -def _pipe_driver_output( - proc: subprocess.Popen, - log_file, - ready_events: "queue.Queue[dict]", -) -> None: - """Copy env_server stdout to log and capture machine-readable ready events.""" - assert proc.stdout is not None - for line in proc.stdout: - log_file.write(line) - log_file.flush() - try: - event = json.loads(line) - except Exception: - continue - if isinstance(event, dict) and event.get("event") == "transport_ready": - ready_events.put(event) - - -def start_env_server( - suite: str, - task: int, - seed: int, - output_dir: str, - max_episode_steps: int = 10000, - libero_type: str | None = None, - cuda_device: str | None = None, - log_path: str | None = None, - driver_script: str | None = None, - ready_timeout_s: float = 300.0, -) -> subprocess.Popen: - """Launch the env server in background. The env server hosts the - env, and prints a machine-readable ``transport_ready`` event on stdout - once its RPC server is listening; this function returns once that event - is seen. - """ - out_dir = Path(output_dir) - out_dir.mkdir(parents=True, exist_ok=True) - - if log_path is None: - log_path = str(out_dir / "env_server.log") - - env = os.environ.copy() - env["LIBERO_TYPE"] = libero_type - if cuda_device is not None: - env["CUDA_VISIBLE_DEVICES"] = str(cuda_device) - env.setdefault("MUJOCO_GL", "egl") - env.setdefault("ROBOT_PLATFORM", "LIBERO") - - cmd = [ - sys.executable, - driver_script or str(get_repo_root() / "robots" / "libero" / "env_server.py"), - "--suite", suite, - "--task", str(task), - "--seed", str(seed), - "--max-episode-steps", str(max_episode_steps), - "--output-dir", str(out_dir), - ] - logger.info("env server cmd: %s", ' '.join(cmd)) - logger.info("env server log: %s", log_path) - logger.info( - "CUDA_VISIBLE_DEVICES=%s output_dir=%s", - env.get("CUDA_VISIBLE_DEVICES"), - out_dir, - ) - log_f = open(log_path, "a") - ready_events: queue.Queue[dict] = queue.Queue() - proc = subprocess.Popen( - cmd, - stdout=subprocess.PIPE, - stderr=subprocess.STDOUT, - env=env, - cwd=get_repo_root(), - text=True, - bufsize=1, - ) - threading.Thread( - target=_pipe_driver_output, - args=(proc, log_f, ready_events), - daemon=True, - ).start() - - logger.info("waiting for env server...") - t0 = time.time() - transport_ready = False - while not transport_ready: - try: - event = ready_events.get(timeout=2.0) - except queue.Empty: - event = None - if event is not None and event.get("kind") == "socket" \ - and event.get("host") and event.get("port"): - set_socket_endpoint(out_dir, event["host"], int(event["port"])) - transport_ready = True - logger.info( - "env server ready at %s:%s", - event["host"], - event["port"], - ) - break - if proc.poll() is not None: - logger.error("env server EXITED before becoming ready. Last log:") - logger.error("%s", Path(log_path).read_text()[-2000:]) - raise RuntimeError("env server exited prematurely") - if time.time() - t0 > ready_timeout_s: - proc.terminate() - raise RuntimeError(f"env server not ready after {ready_timeout_s}s") - logger.info("env server ready in %.1fs", time.time()-t0) - return proc - - -def stop_env_server( - proc: subprocess.Popen, - output_dir: str, - timeout: float = 15.0, -) -> None: - if proc.poll() is not None: - return - try: - client = create_rpc_client(output_dir) - client.call("shutdown", timeout_s=timeout) - except Exception: - pass - try: - proc.wait(timeout=timeout) - except subprocess.TimeoutExpired: - proc.kill() - - -def start_vla_server( - *, - host: str = "127.0.0.1", - port: int = 0, - cuda_device: str | None = None, - log_path: str | None = None, -) -> tuple[str, subprocess.Popen]: - """Launch the Pi0.5 VLA HTTP server in background. - - Returns ``(base_url, proc)``. ``port=0`` asks the OS for a free port. - Caller is responsible for stopping ``proc`` via :func:`stop_vla_server`. - """ - if port == 0: - import socket - with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: - s.bind((host, 0)) - port = int(s.getsockname()[1]) - - env = os.environ.copy() - if cuda_device is not None: - env["CUDA_VISIBLE_DEVICES"] = str(cuda_device) - - cmd = [ - sys.executable, - str(get_repo_root() / "robots" / "libero" / "vla_server.py"), - "--host", host, - "--port", str(port), - ] - logger.info("vla server cmd: %s", " ".join(cmd)) - if log_path: - log_f = open(log_path, "a") - proc = subprocess.Popen(cmd, stdout=log_f, stderr=subprocess.STDOUT, env=env) - else: - proc = subprocess.Popen(cmd, env=env) - - base_url = f"http://{host}:{port}" - # Block until /healthz responds so callers don't race the model load. - client = VLAClient(base_url) - t0 = time.time() - while time.time() - t0 < 300: - if proc.poll() is not None: - raise RuntimeError("vla server exited prematurely") - try: - if client.healthz(): - logger.info("vla server ready at %s after %.1fs", base_url, time.time() - t0) - return base_url, proc - except Exception: - pass - time.sleep(2.0) - proc.terminate() - raise RuntimeError("vla_server not ready after 300s") - - -def stop_vla_server(proc: subprocess.Popen | None, timeout: float = 10.0) -> None: - if proc is None or proc.poll() is not None: - return - proc.terminate() - try: - proc.wait(timeout=timeout) - except subprocess.TimeoutExpired: - proc.kill() - - # --------------------------------------------------------------------------- # API agent transcript serialization # --------------------------------------------------------------------------- @@ -272,8 +71,10 @@ def _strip_images(value): def _serialize_messages(messages: list[dict]) -> list[dict]: """Strip inline image payloads from messages before writing the transcript.""" return [ - {**{k: v for k, v in m.items() if k != "content"}, - "content": _strip_images(m.get("content"))} + { + **{k: v for k, v in m.items() if k != "content"}, + "content": _strip_images(m.get("content")), + } for m in messages ] @@ -289,68 +90,135 @@ def _build_argparser() -> argparse.ArgumentParser: ) # models - ap.add_argument("--cerebrum", default="api", - choices=["api", "claude_code", "codex"], - help="LLM backend: api | claude_code | codex.") - ap.add_argument("--model", default=None, - help="Model id. For the 'api' cerebrum you need to prefix provider to the model id " - "(e.g. anthropic:claude-opus-4-8, openai:gpt-5.5, " - "openai-chat:glm-5.2).") - ap.add_argument("--base-url", default=None, - help="API base URL. Defaults to the selected backend's base URL env var.") - ap.add_argument("--api-key", default=None, - help="API key. Defaults to the selected backend's API key env var.") + ap.add_argument( + "--cerebrum", + default="api", + choices=["api", "claude_code", "codex"], + help="LLM backend: api | claude_code | codex.", + ) + ap.add_argument( + "--model", + default=None, + help="Model id. For the 'api' cerebrum you need to prefix provider to the model id " + "(e.g. anthropic:claude-opus-4-8, openai:gpt-5.5, " + "openai-chat:glm-5.2).", + ) + ap.add_argument( + "--base-url", + default=None, + help="API base URL. Defaults to the selected backend's base URL env var.", + ) + ap.add_argument( + "--api-key", + default=None, + help="API key. Defaults to the selected backend's API key env var.", + ) ap.add_argument("--max-turns", type=int, default=100) ap.add_argument("--max-tokens", type=int, default=8192) - ap.add_argument("--cerebrum-timeout-s", type=int, default=None, - help="Wall-clock cap for the claude_code/codex cerebrum " - "subprocess. Defaults to CODEX_TIMEOUT_S (codex only), " - "CELL_TIMEOUT_S, or 1200.") - ap.add_argument("--claude-code-max-budget-usd", type=float, default=None, - help="Budget passed to claude -p --max-budget-usd. " - "Defaults to MAX_BUDGET_USD env or 10.") + ap.add_argument( + "--cerebrum-timeout-s", + type=int, + default=None, + help="Wall-clock cap for the claude_code/codex cerebrum " + "subprocess. Defaults to CODEX_TIMEOUT_S (codex only), " + "CELL_TIMEOUT_S, or 1200.", + ) + ap.add_argument( + "--claude-code-max-budget-usd", + type=float, + default=None, + help="Budget passed to claude -p --max-budget-usd. " + "Defaults to MAX_BUDGET_USD env or 10.", + ) # env_server / vla_server / transport - ap.add_argument("--no-driver", action="store_true", - help="Don't spawn driver; attach to existing output dir") - ap.add_argument("--env-endpoint", default="127.0.0.1", - help="Host of an existing env server to connect to; required " - "with --no-driver.") - ap.add_argument("--env-port", type=int, default=0, - help="Port of an existing env server to connect to; " - "required with --no-driver.") - ap.add_argument("--vla-endpoint", default=None, - help="Base URL of an existing vla_server (e.g. http://host:8000). " - "If omitted with a spawned driver, a local vla_server is started; " - "required with --no-driver.") - ap.add_argument("--cuda-device", default=None, - help="GPU device(s) to expose via CUDA_VISIBLE_DEVICES.") + ap.add_argument( + "--no-driver", + action="store_true", + help="Don't spawn driver; attach to existing output dir", + ) + ap.add_argument( + "--env-endpoint", + default="127.0.0.1", + help="Host of an existing env server to connect to; required with --no-driver.", + ) + ap.add_argument( + "--env-port", + type=int, + default=0, + help="Port of an existing env server to connect to; required with --no-driver.", + ) + ap.add_argument( + "--vla-endpoint", + default=None, + help="Base URL of an existing vla_server (e.g. http://host:8000). " + "If omitted with a spawned driver, a local vla_server is started; " + "required with --no-driver.", + ) + ap.add_argument( + "--cuda-device", + default=None, + help="GPU device(s) to expose via CUDA_VISIBLE_DEVICES.", + ) # other config ap.add_argument("--output-dir", default=None) - ap.add_argument("--dashboard", action="store_true", - help="Start a local dashboard server for this single run.") - ap.add_argument("--dashboard-host", default="127.0.0.1", - help="Dashboard bind host. Defaults to 127.0.0.1.") - ap.add_argument("--dashboard-port", type=int, default=0, - help="Dashboard port. 0 asks the OS for a free port.") - ap.add_argument("--dashboard-language", choices=["en", "zh-cn"], default="en", - help="Dashboard UI language. 'zh-cn' serves the Chinese " - "variant (index.zh-cn.html); defaults to English.") - ap.add_argument("--verbose", action="store_true", - help="Enable DEBUG-level logging for stdout and the run.log " - "file. Defaults to INFO when not set.") + ap.add_argument( + "--dashboard", + action="store_true", + help="Start a local dashboard server for this single run.", + ) + ap.add_argument( + "--dashboard-host", + default="127.0.0.1", + help="Dashboard bind host. Defaults to 127.0.0.1.", + ) + ap.add_argument( + "--dashboard-port", + type=int, + default=0, + help="Dashboard port. 0 asks the OS for a free port.", + ) + ap.add_argument( + "--dashboard-language", + choices=["en", "zh-cn"], + default="en", + help="Dashboard UI language. 'zh-cn' serves the Chinese " + "variant (index.zh-cn.html); defaults to English.", + ) + ap.add_argument( + "--verbose", + action="store_true", + help="Enable DEBUG-level logging for stdout and the run.log " + "file. Defaults to INFO when not set.", + ) # environments - ap.add_argument("--env", dest="env_name", default="libero", - help="Environment backend. Defaults to libero.") + ap.add_argument( + "--env", + dest="env_name", + default="libero", + help="Environment backend. Defaults to libero.", + ) + ap.add_argument( + "--instruction", + default=None, + help="Natural-language task for physical robot environments.", + ) + ap.add_argument( + "--env-config", default=None, help="Environment-specific configuration file." + ) ap.add_argument("--max-episode-steps", type=int, default=10000) - ap.add_argument("--libero-type", default=None, - choices=["standard", "pro", "plus"], - help="LIBERO variant (auto-routed from suite suffix if not set).") - ap.add_argument("--suite", default=None, - help="e.g. libero_object_task, libero_spatial_swap") + ap.add_argument( + "--libero-type", + default=None, + choices=["standard", "pro", "plus"], + help="LIBERO variant (auto-routed from suite suffix if not set).", + ) + ap.add_argument( + "--suite", default=None, help="e.g. libero_object_task, libero_spatial_swap" + ) ap.add_argument("--task", type=int, default=None) ap.add_argument("--seed", type=int, default=0) @@ -358,6 +226,7 @@ def _build_argparser() -> argparse.ArgumentParser: def main() -> int: + """Run one RPent agent session for the selected environment.""" parser = _build_argparser() args = parser.parse_args() @@ -371,7 +240,8 @@ def main() -> int: from rpent.dashboard.launcher import apply_to_args, defaults_from_args dashboard_server = DashboardServer( - host=args.dashboard_host, port=args.dashboard_port, + host=args.dashboard_host, + port=args.dashboard_port, language=args.dashboard_language, ) dashboard_url = dashboard_server.start() @@ -385,20 +255,13 @@ def main() -> int: apply_to_args(args, launch_config) logger.info("launcher config applied: %s", launch_config) - if not args.suite: - parser.error("--suite is required") - if args.task is None: - parser.error("--task is required") - - suite = args.suite - task = args.task - seed = args.seed env_name = args.env_name + suite = args.suite or env_name + task = args.task if args.task is not None else 0 + seed = args.seed env_spec = get_env_spec(env_name) prompt_bundle = env_spec.prompts - max_episode_steps = args.max_episode_steps - # resolve output directory output_dir = args.output_dir if output_dir is None: @@ -439,13 +302,12 @@ def main() -> int: dashboard=dashboard_state, ) - # Auto-route LIBERO_TYPE if not set - libero_type = args.libero_type or get_libero_type() - prompt_vars = { "suite": suite, "task": task, "seed": seed, + "instruction": args.instruction + or "Inspect the robot and wait for a safe instruction.", "output_dir": output_dir, "recipe_tag": recipe_tag, } @@ -458,72 +320,19 @@ def main() -> int: variables=prompt_vars, ) - env_proc = None - vla_proc = None - vla_endpoint = args.vla_endpoint + runtime = get_runtime( + env_name, + args=args, + output_dir=str(output_dir), + dashboard=dashboard_state, + ) toolkit = None - if not args.no_driver: - env_proc = start_env_server( - suite=suite, task=task, seed=seed, - output_dir=output_dir, - max_episode_steps=max_episode_steps, - cuda_device=args.cuda_device, - libero_type=libero_type, - ) - if vla_endpoint is None: - vla_endpoint, vla_proc = start_vla_server( - cuda_device=args.cuda_device, - log_path=str(Path(output_dir) / "vla_server.log"), - ) - toolkit = get_toolkit( - env_name, - primitives_kwargs={ - "env": LiberoEnvClient( - create_rpc_client(output_dir), - expected_meta={ - "suite": suite, - "task": task, - "seed": seed, - "max_episode_steps": max_episode_steps, - }, - ), - "model": VLAClient(vla_endpoint), - }, - video_path=str(Path(output_dir) / "episode.mp4"), - dashboard=dashboard_state, - ) - else: - if args.env_port <= 0: - raise RuntimeError( - "--no-driver requires --env-port pointing at an existing env_server" - ) - if vla_endpoint is None: - raise RuntimeError( - "--no-driver requires --vla-endpoint pointing at an existing vla_server" - ) - set_socket_endpoint(output_dir, args.env_endpoint, args.env_port) - toolkit = get_toolkit( - env_name, - primitives_kwargs={ - "env": LiberoEnvClient( - create_rpc_client(output_dir), - expected_meta={ - "suite": suite, - "task": task, - "seed": seed, - "max_episode_steps": max_episode_steps, - }, - ), - "model": VLAClient(vla_endpoint), - }, - video_path=str(Path(output_dir) / "episode.mp4"), - dashboard=dashboard_state, - ) t0 = time.time() finish_result, messages, agent_error = None, [], None stats: dict = {} try: + toolkit = runtime.start() result = cerebrum.solve( system_prompt=system_prompt, user_message=user_msg, @@ -537,21 +346,20 @@ def main() -> int: except Exception as e: logger.error("EXCEPTION in agent loop: %s", e) finally: - # Agent-side: flush the episode video before the env+model - recipe_path = toolkit.write_recipe(recipe_tag) - logger.info("recipe: %s", recipe_path) - - toolkit.close() - if env_proc is not None: - stop_env_server(env_proc, output_dir=output_dir) - if vla_proc is not None: - stop_vla_server(vla_proc) + if toolkit is not None: + recipe_path = toolkit.write_recipe(recipe_tag) + logger.info("recipe: %s", recipe_path) + toolkit.close() + runtime.stop() elapsed = time.time() - t0 transcript_path = Path(output_dir) / f"transcript_{recipe_tag}.json" record = { - "suite": suite, "task": task, "seed": seed, "model": args.model, + "suite": suite, + "task": task, + "seed": seed, + "model": args.model, "elapsed_s": round(elapsed, 1), "finish": finish_result, "stats": stats, @@ -561,10 +369,12 @@ def main() -> int: json.dump(record, f, indent=2, default=str) logger.info("elapsed: %.1fs", elapsed) - logger.info("usage: in=%s out=%s tool_calls=%s", - stats.get('total_input_tokens', '?'), - stats.get('total_output_tokens', '?'), - stats.get('tool_calls', '?')) + logger.info( + "usage: in=%s out=%s tool_calls=%s", + stats.get("total_input_tokens", "?"), + stats.get("total_output_tokens", "?"), + stats.get("tool_calls", "?"), + ) logger.info("transcript: %s", transcript_path) if agent_error: logger.error("error: %s", agent_error) diff --git a/rpent/envs/__init__.py b/rpent/envs/__init__.py index 5a03bb9e..d8726f37 100644 --- a/rpent/envs/__init__.py +++ b/rpent/envs/__init__.py @@ -1,12 +1,15 @@ """Environment-specific RPent extensions.""" +from rpent.envs.base import get_env_spec, get_runtime, get_toolkit from rpent.envs.env_spec import EnvSpec from rpent.envs.prompt_bundle import PromptBundle -from rpent.envs.base import get_env_spec, get_toolkit +from rpent.envs.runtime import EnvRuntime __all__ = [ "EnvSpec", + "EnvRuntime", "PromptBundle", "get_env_spec", + "get_runtime", "get_toolkit", ] diff --git a/rpent/envs/base.py b/rpent/envs/base.py index cbe288e9..f381cdca 100644 --- a/rpent/envs/base.py +++ b/rpent/envs/base.py @@ -6,6 +6,7 @@ so cerebrums and envs share the same contract types without crossing module layers. """ + from __future__ import annotations import importlib @@ -36,9 +37,19 @@ def _resolve_env(name: str) -> Any: def get_env_spec(name: str) -> EnvSpec: + """Return the static descriptor exposed by ``robots.``.""" return _resolve_env(name).get_env_spec() def get_toolkit(name: str, **kwargs) -> Toolkit: """Build the env toolkit (common tools + env-specific tools).""" return _resolve_env(name).get_toolkit(**kwargs) + + +def get_runtime(name: str, **kwargs): + """Build the lifecycle adapter exposed by ``robots.``.""" + module = _resolve_env(name) + factory = getattr(module, "get_runtime", None) + if factory is None: + raise ValueError(f"environment {name!r} does not expose get_runtime") + return factory(**kwargs) diff --git a/rpent/envs/process.py b/rpent/envs/process.py new file mode 100644 index 00000000..54a205d1 --- /dev/null +++ b/rpent/envs/process.py @@ -0,0 +1,113 @@ +"""Shared subprocess lifecycle helpers for socket-based environment servers.""" + +from __future__ import annotations + +import json +import os +import queue +import subprocess +import threading +import time +from pathlib import Path +from typing import TextIO + +from rpent.utils.rpc import create_rpc_client, set_socket_endpoint + + +def _pipe_output( + proc: subprocess.Popen, + log_file: TextIO, + ready_events: "queue.Queue[dict]", +) -> None: + assert proc.stdout is not None + try: + for line in proc.stdout: + log_file.write(line) + log_file.flush() + try: + event = json.loads(line) + except Exception: + continue + if isinstance(event, dict) and event.get("event") == "transport_ready": + ready_events.put(event) + finally: + log_file.close() + + +def start_socket_server_process( + command: list[str], + *, + output_dir: str | Path, + log_name: str, + env: dict[str, str] | None = None, + cwd: str | Path | None = None, + ready_timeout_s: float = 300.0, +) -> subprocess.Popen: + """Start a server and register the socket endpoint from its ready event.""" + out_dir = Path(output_dir) + out_dir.mkdir(parents=True, exist_ok=True) + log_file = (out_dir / log_name).open("a", encoding="utf-8") + proc = subprocess.Popen( + command, + stdout=subprocess.PIPE, + stderr=subprocess.STDOUT, + text=True, + bufsize=1, + env=env or os.environ.copy(), + cwd=cwd, + ) + ready_events: queue.Queue[dict] = queue.Queue() + threading.Thread( + target=_pipe_output, + args=(proc, log_file, ready_events), + daemon=True, + ).start() + + deadline = time.monotonic() + ready_timeout_s + while True: + try: + event = ready_events.get(timeout=0.2) + except queue.Empty: + event = None + if ( + event is not None + and event.get("kind") == "socket" + and event.get("host") + and event.get("port") + ): + set_socket_endpoint(out_dir, event["host"], int(event["port"])) + return proc + if proc.poll() is not None: + tail = (out_dir / log_name).read_text(errors="replace")[-3000:] + raise RuntimeError( + f"environment server exited before becoming ready:\n{tail}" + ) + if time.monotonic() >= deadline: + proc.terminate() + raise RuntimeError( + f"environment server not ready after {ready_timeout_s:.1f}s" + ) + + +def stop_socket_server_process( + proc: subprocess.Popen | None, + *, + output_dir: str | Path, + timeout_s: float = 15.0, +) -> None: + """Request graceful shutdown, then terminate a stuck server.""" + if proc is None or proc.poll() is not None: + return + try: + create_rpc_client(output_dir).call("shutdown", timeout_s=timeout_s) + except Exception: + pass + try: + proc.wait(timeout=timeout_s) + return + except subprocess.TimeoutExpired: + proc.terminate() + try: + proc.wait(timeout=5.0) + except subprocess.TimeoutExpired: + proc.kill() diff --git a/rpent/envs/runtime.py b/rpent/envs/runtime.py new file mode 100644 index 00000000..66e60aa2 --- /dev/null +++ b/rpent/envs/runtime.py @@ -0,0 +1,19 @@ +"""Lifecycle contract for environment-specific runtime adapters.""" + +from __future__ import annotations + +from abc import ABC, abstractmethod + +from rpent.tools.toolkit import Toolkit + + +class EnvRuntime(ABC): + """Own the processes and clients needed by one RPent environment.""" + + @abstractmethod + def start(self) -> Toolkit: + """Start or attach to the environment and return its agent toolkit.""" + + @abstractmethod + def stop(self) -> None: + """Release environment processes and transport resources.""" diff --git a/scripts/check_rebot_robstride.py b/scripts/check_rebot_robstride.py new file mode 100644 index 00000000..b02f956d --- /dev/null +++ b/scripts/check_rebot_robstride.py @@ -0,0 +1,36 @@ +#!/usr/bin/env python3 +"""Passive seven-motor connectivity check for a reBot DevArm RobStride arm. + +This script only reads RobStride mechPos/mechVel parameters. It never clears +faults, changes modes, enables motors, or sends a target. +""" + +from __future__ import annotations + +import argparse +import json +from pathlib import Path + +from robots.rebot_robstride.config import load_config +from robots.rebot_robstride.driver import RebotRobstrideDriver +from robots.rebot_robstride.env_server import validate_socketcan + + +def main() -> int: + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--config", type=Path, default=None) + args = parser.parse_args() + + config = load_config(args.config) + validate_socketcan(config.channel, config.bitrate) + driver = RebotRobstrideDriver(config) + try: + state = driver.connect() + print(json.dumps(state, indent=2, sort_keys=True)) + finally: + driver.close() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tests/envs/test_runtime_registry.py b/tests/envs/test_runtime_registry.py new file mode 100644 index 00000000..82670231 --- /dev/null +++ b/tests/envs/test_runtime_registry.py @@ -0,0 +1,44 @@ +from __future__ import annotations + +import sys +import types + +import pytest + +from rpent.envs import get_runtime +from rpent.envs.runtime import EnvRuntime + + +class DummyRuntime(EnvRuntime): + def __init__(self, marker: str) -> None: + self.marker = marker + self.started = False + self.stopped = False + + def start(self): + self.started = True + return object() + + def stop(self) -> None: + self.stopped = True + + +def test_get_runtime_uses_lazy_environment_factory( + monkeypatch: pytest.MonkeyPatch, +) -> None: + module = types.ModuleType("robots.fake_runtime") + module.get_runtime = lambda **kwargs: DummyRuntime(kwargs["marker"]) + monkeypatch.setitem(sys.modules, "robots.fake_runtime", module) + + runtime = get_runtime("fake_runtime", marker="expected") + + assert isinstance(runtime, DummyRuntime) + assert runtime.marker == "expected" + + +def test_get_runtime_reports_missing_factory(monkeypatch: pytest.MonkeyPatch) -> None: + module = types.ModuleType("robots.no_runtime") + monkeypatch.setitem(sys.modules, "robots.no_runtime", module) + + with pytest.raises(ValueError, match="does not expose get_runtime"): + get_runtime("no_runtime") diff --git a/tests/robots/rebot_robstride/test_config.py b/tests/robots/rebot_robstride/test_config.py new file mode 100644 index 00000000..0277fd18 --- /dev/null +++ b/tests/robots/rebot_robstride/test_config.py @@ -0,0 +1,60 @@ +from __future__ import annotations + +from pathlib import Path + +import pytest + +from robots.rebot_robstride.config import default_config, load_config + + +def test_default_config_matches_rebot_robstride_bus() -> None: + config = default_config() + assert config.channel == "can0" + assert config.bitrate == 1_000_000 + assert [joint.motor_id for joint in config.joints] == [1, 2, 3, 4, 5, 6] + assert [joint.model for joint in config.joints] == [ + "rs-06", + "rs-06", + "rs-06", + "rs-00", + "rs-00", + "rs-00", + ] + assert config.gripper.motor_id == 7 + assert config.gripper.model == "rs-00" + assert config.gripper.open_position is None + assert config.gripper.closed_position is None + + +def test_load_config_rejects_duplicate_motor_ids(tmp_path: Path) -> None: + path = tmp_path / "duplicate.yaml" + path.write_text( + """ +channel: can0 +joints: + - {name: joint1, motor_id: 1, model: rs-06, lower: -2.8, upper: 2.8, kp: 20, kd: 1} + - {name: joint2, motor_id: 1, model: rs-06, lower: -3.14, upper: 0, kp: 20, kd: 1} + - {name: joint3, motor_id: 3, model: rs-06, lower: -3.14, upper: 0, kp: 20, kd: 1} + - {name: joint4, motor_id: 4, model: rs-00, lower: -1.57, upper: 1.57, kp: 15, kd: 1} + - {name: joint5, motor_id: 5, model: rs-00, lower: -1.57, upper: 1.57, kp: 15, kd: 1} + - {name: joint6, motor_id: 6, model: rs-00, lower: -3.14, upper: 3.14, kp: 15, kd: 1} +gripper: {motor_id: 7, model: rs-00} +""".strip() + ) + + with pytest.raises(ValueError, match="motor IDs must be unique"): + load_config(path) + + +def test_load_config_rejects_half_calibrated_gripper(tmp_path: Path) -> None: + path = tmp_path / "gripper.yaml" + path.write_text( + """ +gripper: + open_position: -5.0 + closed_position: +""".strip() + ) + + with pytest.raises(ValueError, match="both be set or both be null"): + load_config(path) diff --git a/tests/robots/rebot_robstride/test_driver.py b/tests/robots/rebot_robstride/test_driver.py new file mode 100644 index 00000000..22fc71ef --- /dev/null +++ b/tests/robots/rebot_robstride/test_driver.py @@ -0,0 +1,276 @@ +from __future__ import annotations + +from dataclasses import replace + +import pytest + +from robots.rebot_robstride.config import default_config +from robots.rebot_robstride.driver import RebotRobstrideDriver + +MECH_POS = 0x7019 +MECH_VEL = 0x701A + + +class FakeMotor: + def __init__(self, motor_id: int, position: float = 0.0) -> None: + self.motor_id = motor_id + self.position = position + self.velocity = 0.0 + self.calls: list[tuple] = [] + self.fail_on_send = False + + def robstride_get_param_f32(self, parameter: int, timeout_ms: int = 1000) -> float: + self.calls.append(("read", parameter, timeout_ms)) + if parameter == MECH_POS: + return self.position + if parameter == MECH_VEL: + return self.velocity + raise AssertionError(f"unexpected parameter {parameter:#x}") + + def clear_error(self) -> None: + self.calls.append(("clear_error",)) + + def ensure_mode(self, mode, timeout_ms: int = 1000) -> None: + self.calls.append(("ensure_mode", int(mode), timeout_ms)) + + def send_mit( + self, pos: float, vel: float, kp: float, kd: float, tau: float + ) -> None: + if self.fail_on_send: + raise RuntimeError("injected send failure") + self.calls.append(("send_mit", pos, vel, kp, kd, tau)) + self.velocity = pos - self.position + self.position = pos + + +class FakeController: + def __init__(self, channel: str, positions: dict[int, float] | None = None) -> None: + self.channel = channel + self.positions = positions or {} + self.motors: dict[int, FakeMotor] = {} + self.enabled = False + self.disabled = False + self.closed = False + + def add_robstride_motor( + self, motor_id: int, feedback_id: int, model: str + ) -> FakeMotor: + assert feedback_id == 0xFD + motor = FakeMotor(motor_id, self.positions.get(motor_id, 0.0)) + motor.calls.append(("registered", feedback_id, model)) + self.motors[motor_id] = motor + return motor + + def enable_all(self) -> None: + self.enabled = True + + def disable_all(self) -> None: + self.disabled = True + self.enabled = False + + def close(self) -> None: + self.closed = True + + +def make_driver(positions: dict[int, float] | None = None): + controllers: list[FakeController] = [] + + def factory(channel: str) -> FakeController: + controller = FakeController(channel, positions) + controllers.append(controller) + return controller + + driver = RebotRobstrideDriver( + default_config(), + controller_factory=factory, + sleep=lambda _: None, + ) + return driver, controllers + + +def test_connect_is_passive_and_reads_every_motor() -> None: + expected = {motor_id: motor_id / 100 for motor_id in range(1, 8)} + driver, controllers = make_driver(expected) + + state = driver.connect() + + controller = controllers[0] + assert controller.channel == "can0" + assert sorted(controller.motors) == list(range(1, 8)) + assert not controller.enabled + assert state["joint_positions"] == pytest.approx([expected[i] for i in range(1, 7)]) + assert state["gripper_position"] == pytest.approx(expected[7]) + for motor in controller.motors.values(): + assert any(call[:2] == ("read", MECH_POS) for call in motor.calls) + + driver.close() + assert controller.disabled is False + assert controller.closed is True + + +def test_enable_holds_observed_pose_before_accepting_motion() -> None: + positions = {motor_id: -0.01 * motor_id for motor_id in range(1, 8)} + driver, controllers = make_driver(positions) + driver.connect() + + result = driver.enable() + + controller = controllers[0] + assert result["enabled"] is True + assert controller.enabled + for motor_id, motor in controller.motors.items(): + names = [call[0] for call in motor.calls] + assert "clear_error" in names + assert "ensure_mode" in names + hold = next(call for call in motor.calls if call[0] == "send_mit") + assert hold[1] == pytest.approx(positions[motor_id]) + + +def test_enable_failure_disables_all_motors() -> None: + driver, controllers = make_driver() + driver.connect() + controllers[0].motors[1].fail_on_send = True + + with pytest.raises(RuntimeError, match="injected send failure"): + driver.enable() + + assert controllers[0].disabled is True + assert driver.state()["enabled"] is False + + +def test_move_joints_interpolates_and_returns_readback_evidence() -> None: + driver, controllers = make_driver() + driver.connect() + driver.enable() + target = [0.1, -0.1, -0.1, 0.05, -0.05, 0.1] + + result = driver.move_joints(target, duration_s=0.2) + + assert result["reached"] is True + assert result["final_positions"] == pytest.approx(target) + assert result["max_error"] == pytest.approx(0.0) + for motor_id in range(1, 7): + commands = [ + call + for call in controllers[0].motors[motor_id].calls + if call[0] == "send_mit" + ] + assert len(commands) >= 3 # initial hold plus at least two trajectory points + + +def test_motion_transport_failure_latches_soft_stop() -> None: + driver, controllers = make_driver() + driver.connect() + driver.enable() + controllers[0].motors[3].fail_on_send = True + + with pytest.raises(RuntimeError, match="injected send failure"): + driver.move_joints([0.1, -0.1, -0.1, 0.0, 0.0, 0.0], duration_s=1.0) + + state = driver.state() + assert state["enabled"] is True + assert state["stopped"] is True + with pytest.raises(RuntimeError, match="stopped"): + driver.move_joints([0.0] * 6, duration_s=1.0) + + +def test_move_joints_rejects_limit_violation() -> None: + driver, _ = make_driver() + driver.connect() + driver.enable() + + with pytest.raises(ValueError, match="joint2"): + driver.move_joints([0.0, 0.1, -0.1, 0.0, 0.0, 0.0], duration_s=1.0) + + +def test_motion_requires_enable_and_respects_soft_stop() -> None: + driver, _ = make_driver() + driver.connect() + + with pytest.raises(RuntimeError, match="not enabled"): + driver.move_joints([0.0] * 6, duration_s=1.0) + + driver.enable() + driver.stop_motion() + with pytest.raises(RuntimeError, match="stopped"): + driver.move_joints([0.0] * 6, duration_s=1.0) + driver.reset_stop() + assert driver.move_joints([0.0] * 6, duration_s=0.1)["reached"] is True + + +def test_stop_failure_keeps_soft_stop_latched() -> None: + driver, controllers = make_driver() + driver.connect() + driver.enable() + controllers[0].motors[3].fail_on_send = True + + with pytest.raises(RuntimeError, match="injected send failure"): + driver.stop_motion() + + assert driver.state()["stopped"] is True + + +def test_emergency_stop_disables_all_motors() -> None: + driver, controllers = make_driver() + driver.connect() + driver.enable() + + result = driver.emergency_stop() + + assert result == {"enabled": False, "stopped": True} + assert controllers[0].disabled + + +def test_gripper_refuses_motion_until_endpoints_are_calibrated() -> None: + driver, _ = make_driver() + driver.connect() + driver.enable() + + with pytest.raises(RuntimeError, match="not calibrated"): + driver.set_gripper(0.5) + + +def test_gripper_maps_normalized_position_to_calibrated_range() -> None: + config = default_config() + calibrated = replace( + config, + gripper=replace(config.gripper, open_position=-1.0, closed_position=0.0), + ) + controller = FakeController("can0") + driver = RebotRobstrideDriver( + calibrated, + controller_factory=lambda _: controller, + sleep=lambda _: None, + ) + driver.connect() + driver.enable() + + with pytest.raises(ValueError, match="duration_s"): + driver.set_gripper(0.25, duration_s=0.0) + + result = driver.set_gripper(0.25) + + assert result["target_position"] == pytest.approx(-0.75) + assert controller.motors[7].position == pytest.approx(-0.75) + + +def test_gripper_transport_failure_latches_soft_stop() -> None: + config = default_config() + calibrated = replace( + config, + gripper=replace(config.gripper, open_position=-1.0, closed_position=0.0), + ) + controller = FakeController("can0") + driver = RebotRobstrideDriver( + calibrated, + controller_factory=lambda _: controller, + sleep=lambda _: None, + ) + driver.connect() + driver.enable() + controller.motors[7].fail_on_send = True + + with pytest.raises(RuntimeError, match="injected send failure"): + driver.set_gripper(0.5) + + assert driver.state()["stopped"] is True diff --git a/tests/robots/rebot_robstride/test_env_client.py b/tests/robots/rebot_robstride/test_env_client.py new file mode 100644 index 00000000..81ec7ece --- /dev/null +++ b/tests/robots/rebot_robstride/test_env_client.py @@ -0,0 +1,30 @@ +from __future__ import annotations + +from robots.rebot_robstride.env_client import RebotRobstrideEnvClient + + +class FakeRpcClient: + def __init__(self) -> None: + self.calls: list[tuple] = [] + + def call(self, method, args=(), kwargs=None, timeout_s=None): + self.calls.append((method, args, kwargs or {}, timeout_s)) + return {"method": method, **(kwargs or {})} + + +def test_client_uses_stable_robot_rpc_names() -> None: + rpc = FakeRpcClient() + client = RebotRobstrideEnvClient(rpc) + + assert client.state()["method"] == "robot.state" + assert client.enable()["method"] == "robot.enable" + move = client.move_joints([0.0] * 6, duration_s=3.0) + assert move == { + "method": "robot.move_joints", + "positions": [0.0] * 6, + "duration_s": 3.0, + } + assert client.set_gripper(0.5, duration_s=1.5)["method"] == "robot.set_gripper" + assert client.stop_motion()["method"] == "robot.stop_motion" + assert client.reset_stop()["method"] == "robot.reset_stop" + assert client.emergency_stop()["method"] == "robot.emergency_stop" diff --git a/tests/robots/rebot_robstride/test_env_server.py b/tests/robots/rebot_robstride/test_env_server.py new file mode 100644 index 00000000..9fbe86cd --- /dev/null +++ b/tests/robots/rebot_robstride/test_env_server.py @@ -0,0 +1,64 @@ +from __future__ import annotations + +import threading + +import pytest + +from robots.rebot_robstride.env_server import make_dispatch + + +class FakeDriver: + def state(self): + return {"op": "state"} + + def enable(self): + return {"op": "enable"} + + def move_joints(self, positions, *, duration_s=2.0): + return {"op": "move", "positions": positions, "duration_s": duration_s} + + def set_gripper(self, position, *, duration_s=1.0): + return {"op": "gripper", "position": position, "duration_s": duration_s} + + def stop_motion(self): + return {"op": "stop"} + + def reset_stop(self): + return {"op": "reset"} + + def emergency_stop(self): + return {"op": "estop"} + + +def test_dispatch_maps_only_declared_robot_methods() -> None: + shutdown = threading.Event() + dispatch = make_dispatch(FakeDriver(), shutdown) + + assert dispatch("robot.state", (), {}) == {"op": "state"} + assert dispatch("robot.enable", (), {}) == {"op": "enable"} + assert ( + dispatch("robot.move_joints", (), {"positions": [0.0] * 6, "duration_s": 3.0})[ + "duration_s" + ] + == 3.0 + ) + assert ( + dispatch("robot.set_gripper", (), {"position": 0.5, "duration_s": 1.5})[ + "position" + ] + == 0.5 + ) + assert dispatch("robot.stop_motion", (), {}) == {"op": "stop"} + assert dispatch("robot.reset_stop", (), {}) == {"op": "reset"} + assert dispatch("robot.emergency_stop", (), {}) == {"op": "estop"} + + with pytest.raises(ValueError, match="unknown RPC method"): + dispatch("robot.set_zero", (), {}) + + +def test_shutdown_sets_event_without_touching_motion() -> None: + shutdown = threading.Event() + dispatch = make_dispatch(FakeDriver(), shutdown) + + assert dispatch("shutdown", (), {}) == {"ok": True} + assert shutdown.is_set() diff --git a/tests/robots/rebot_robstride/test_toolkit.py b/tests/robots/rebot_robstride/test_toolkit.py new file mode 100644 index 00000000..fc6fd343 --- /dev/null +++ b/tests/robots/rebot_robstride/test_toolkit.py @@ -0,0 +1,72 @@ +from __future__ import annotations + +from robots.rebot_robstride.toolkit import RebotRobstrideToolkit + + +class FakeEnv: + def __init__(self) -> None: + self.calls: list[tuple] = [] + + def state(self): + self.calls.append(("state",)) + return {"enabled": False, "joint_positions": [0.0] * 6} + + def enable(self): + self.calls.append(("enable",)) + return {"enabled": True} + + def move_joints(self, positions, *, duration_s=2.0): + self.calls.append(("move_joints", positions, duration_s)) + return {"reached": True, "final_positions": positions} + + def set_gripper(self, position, *, duration_s=1.0): + self.calls.append(("set_gripper", position, duration_s)) + return {"normalized_position": position} + + def stop_motion(self): + self.calls.append(("stop_motion",)) + return {"stopped": True} + + def reset_stop(self): + self.calls.append(("reset_stop",)) + return {"stopped": False} + + def emergency_stop(self): + self.calls.append(("emergency_stop",)) + return {"enabled": False, "stopped": True} + + +def test_toolkit_exposes_safe_rebot_tools() -> None: + toolkit = RebotRobstrideToolkit(env=FakeEnv()) + + names = [spec["name"] for spec in toolkit.get_tools_spec()] + + assert { + "get_robot_state", + "enable_arm", + "move_joints", + "set_gripper", + "open_gripper", + "close_gripper", + "stop_motion", + "reset_stop", + "emergency_stop", + }.issubset(names) + + +def test_toolkit_dispatches_motion_and_gripper_without_bypassing_client() -> None: + env = FakeEnv() + toolkit = RebotRobstrideToolkit(env=env) + + move = toolkit.execute_tool( + "move_joints", {"positions": [0.0] * 6, "duration_s": 3.0} + ) + opened = toolkit.execute_tool("open_gripper", {"duration_s": 1.5}) + closed = toolkit.execute_tool("close_gripper", {}) + + assert move.result["reached"] is True + assert opened.result["normalized_position"] == 0.0 + assert closed.result["normalized_position"] == 1.0 + assert ("move_joints", [0.0] * 6, 3.0) in env.calls + assert ("set_gripper", 0.0, 1.5) in env.calls + assert ("set_gripper", 1.0, 1.0) in env.calls diff --git a/tests/robots/test_runtimes.py b/tests/robots/test_runtimes.py new file mode 100644 index 00000000..a6be468c --- /dev/null +++ b/tests/robots/test_runtimes.py @@ -0,0 +1,90 @@ +from __future__ import annotations + +from argparse import Namespace +from typing import cast + +import robots.libero.runtime as libero_runtime_module +import robots.rebot_robstride.runtime as rebot_runtime_module + + +class FakeToolkit: + def __init__(self, **kwargs) -> None: + self.kwargs = kwargs + + +class FakeLiberoEnv: + def __init__(self, rpc, *, expected_meta) -> None: + self.rpc = rpc + self.expected_meta = expected_meta + + +def test_libero_runtime_preserves_no_driver_attach(monkeypatch, tmp_path) -> None: + endpoint_calls: list[tuple] = [] + fake_rpc = object() + monkeypatch.setattr( + libero_runtime_module, + "set_socket_endpoint", + lambda output, host, port: endpoint_calls.append((output, host, port)), + ) + monkeypatch.setattr(libero_runtime_module, "create_rpc_client", lambda _: fake_rpc) + monkeypatch.setattr(libero_runtime_module, "LiberoEnvClient", FakeLiberoEnv) + monkeypatch.setattr(libero_runtime_module, "VLAClient", lambda url: ("vla", url)) + monkeypatch.setattr(libero_runtime_module, "LiberoToolkit", FakeToolkit) + args = Namespace( + suite="libero_object_task", + task=2, + seed=3, + max_episode_steps=99, + no_driver=True, + env_endpoint="127.0.0.1", + env_port=45001, + vla_endpoint="http://127.0.0.1:45002", + libero_type=None, + cuda_device=None, + ) + + runtime = libero_runtime_module.LiberoRuntime( + args=args, output_dir=tmp_path, dashboard=None + ) + toolkit = cast(FakeToolkit, runtime.start()) + + assert endpoint_calls == [(tmp_path, "127.0.0.1", 45001)] + assert toolkit.kwargs["primitives_kwargs"]["env"].expected_meta == { + "suite": "libero_object_task", + "task": 2, + "seed": 3, + "max_episode_steps": 99, + } + assert toolkit.kwargs["primitives_kwargs"]["model"] == ( + "vla", + "http://127.0.0.1:45002", + ) + + +def test_rebot_runtime_supports_no_driver_attach(monkeypatch, tmp_path) -> None: + endpoint_calls: list[tuple] = [] + fake_rpc = object() + monkeypatch.setattr( + rebot_runtime_module, + "set_socket_endpoint", + lambda output, host, port: endpoint_calls.append((output, host, port)), + ) + monkeypatch.setattr(rebot_runtime_module, "create_rpc_client", lambda _: fake_rpc) + monkeypatch.setattr( + rebot_runtime_module, "RebotRobstrideEnvClient", lambda rpc: ("env", rpc) + ) + monkeypatch.setattr(rebot_runtime_module, "RebotRobstrideToolkit", FakeToolkit) + args = Namespace( + no_driver=True, + env_endpoint="127.0.0.1", + env_port=46001, + env_config=None, + ) + + runtime = rebot_runtime_module.RebotRobstrideRuntime( + args=args, output_dir=tmp_path, dashboard=None + ) + toolkit = cast(FakeToolkit, runtime.start()) + + assert endpoint_calls == [(tmp_path, "127.0.0.1", 46001)] + assert toolkit.kwargs["env"] == ("env", fake_rpc) From c4b3572dbbd3949be9581539ece8ab0755c17732 Mon Sep 17 00:00:00 2001 From: johnnynunez Date: Mon, 20 Jul 2026 04:39:48 +0200 Subject: [PATCH 2/6] fix(rebot): harden real-hardware safety Add preemptive stop RPCs, monitored fail-closed trajectories, startup guards, an agent heartbeat deadman, strict configuration validation, and safe shutdown behavior. Also fix LIBERO CLI validation, package robot backends and config data in wheels, and extend concurrency, lifecycle, packaging, and safety regression coverage. --- .../rst_source/usage/rebot_robstride.rst | 46 +- pyproject.toml | 6 +- robots/libero/__init__.py | 6 + robots/rebot_robstride/config.py | 125 ++- .../config/rebot_robstride.example.yaml | 10 +- robots/rebot_robstride/driver.py | 752 ++++++++++++++---- robots/rebot_robstride/env_client.py | 5 +- robots/rebot_robstride/env_server.py | 55 +- robots/rebot_robstride/prompts/system.py | 5 +- robots/rebot_robstride/runtime.py | 32 + robots/rebot_robstride/toolkit.py | 30 +- rpent/cli/main.py | 7 +- rpent/envs/__init__.py | 3 +- rpent/envs/base.py | 13 +- rpent/utils/socket_rpc.py | 14 +- scripts/check_rebot_robstride.py | 4 +- tests/envs/test_runtime_registry.py | 20 +- tests/robots/rebot_robstride/test_config.py | 35 + tests/robots/rebot_robstride/test_driver.py | 378 +++++++-- .../robots/rebot_robstride/test_env_client.py | 1 + .../robots/rebot_robstride/test_env_server.py | 19 +- tests/robots/test_runtimes.py | 64 +- tests/utils/test_socket_rpc_priority.py | 46 ++ 23 files changed, 1380 insertions(+), 296 deletions(-) create mode 100644 tests/utils/test_socket_rpc_priority.py diff --git a/docs/source-en/rst_source/usage/rebot_robstride.rst b/docs/source-en/rst_source/usage/rebot_robstride.rst index 5c7366a1..47d7de93 100644 --- a/docs/source-en/rst_source/usage/rebot_robstride.rst +++ b/docs/source-en/rst_source/usage/rebot_robstride.rst @@ -36,9 +36,11 @@ Verify all seven motors before enabling torque: python scripts/check_rebot_robstride.py -The check only reads RobStride ``mechPos`` (``0x7019``) and ``mechVel`` -(``0x701A``) parameters. It never clears faults, changes modes, enables motors, -or sends a target. +The check only reads RobStride ``mechPos`` (``0x7019``) and fault/warning +reports. It never clears faults, changes modes, enables motors, or sends a +target. Runtime velocity feedback is estimated from timestamped position +samples because ``mechVel`` scaling is not consistent across the tested +RobStride firmware variants. Configuration ------------- @@ -86,14 +88,17 @@ Start a physical-agent run with an explicit natural-language instruction: --model anthropic:claude-opus-4-8 The arm starts disabled. An agent must call ``get_robot_state`` and then -``enable_arm`` before any motion. ``enable_arm`` clears motor errors, selects -MIT mode, enables the controller, and immediately holds the observed pose to -avoid a startup jump. +``enable_arm`` before any motion. ``enable_arm`` first validates raw joint +limits and near-zero startup velocity, clears faults, verifies they are clear, +selects MIT mode, enables only the six arm motors, and immediately holds the +observed pose. The gripper remains disabled until a calibrated gripper command +explicitly enables it. Available robot tools --------------------- -- ``get_robot_state`` — fresh positions and velocities for all motors. +- ``get_robot_state`` — fresh positions, estimated velocities, and raw + fault/warning reports for all motors. - ``enable_arm`` — explicit fault-clear, mode selection, enable, and pose hold. - ``move_joints`` — six raw-motor-radian targets through a bounded minimum-jerk trajectory with final read-back evidence. @@ -107,13 +112,34 @@ Safety limits ------------- ``move_joints`` rejects non-finite values, targets outside the configured joint -limits, and malformed six-joint vectors. Requested durations are stretched when -needed to respect each joint's configured velocity cap. Every result includes -the target, final hardware read-back, maximum error, and ``reached`` flag. +limits, malformed six-joint vectors, and durations above the configured hard +maximum. Requested durations are stretched by the 1.875 peak derivative of the +minimum-jerk profile so the actual setpoint velocity respects each configured +cap. During motion the driver polls position and fault feedback, aborting and +disabling all motors on a transport error, fault/warning report, excessive +tracking error, or excessive measured velocity. Completion requires consecutive +position-and-velocity settlement samples. Gripper results use the same +``target``/``final``/``max_error``/``reached`` evidence contract. + +``robot.stop_motion``, ``robot.emergency_stop``, heartbeats, and ``shutdown`` +use a priority RPC path that bypasses the serialized motion-command lock. +Trajectories check a cancellation latch at every control tick. A runtime worker +sends periodic heartbeats; if the agent process disappears while torque is +enabled, the server calls emergency stop after ``heartbeat_timeout_s``. + +The pickle-framed hardware RPC server accepts loopback binds only. Do not expose +it through a TCP proxy or bind it to a LAN address. ``emergency_stop`` removes torque; an unsupported arm may fall under gravity. Support the arm and keep the physical emergency stop accessible during initial hardware validation. +Normal shutdown always disables enabled motors; there is no unattended ``hold`` +shutdown mode. The heartbeat protects loss of the agent process while the +hardware server remains alive. A hard crash or ``SIGKILL`` of the hardware +server cannot be proven fail-safe because the tested RobStride/motorbridge API +does not expose a confirmed hardware watchdog. Never operate unattended, and +keep the physical emergency stop reachable. + The first implementation provides guarded joint-space control. It does not claim collision avoidance, Cartesian planning, or perception-based grasping. diff --git a/pyproject.toml b/pyproject.toml index c25fa4be..14ddfae4 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -62,9 +62,13 @@ include-package-data = true [tool.setuptools.packages.find] where = ["."] -include = ["rpent*"] +include = ["rpent*", "robots*"] exclude = ["tests*", "docs*", "examples*", "docker*", "toolkits*"] +[tool.setuptools.package-data] +"robots.libero" = ["README.md", "guides/*.md", "prompts/*.md"] +"robots.rebot_robstride" = ["config/*.yaml"] + [tool.ruff] line-length = 88 indent-width = 4 diff --git a/robots/libero/__init__.py b/robots/libero/__init__.py index 4ac10154..ebcaeda0 100644 --- a/robots/libero/__init__.py +++ b/robots/libero/__init__.py @@ -12,6 +12,12 @@ from rpent.envs.prompt_bundle import PromptBundle +def validate_args(args: Any, parser: Any) -> None: + """Reject incomplete LIBERO invocations during CLI parsing.""" + if args.suite is None or args.task is None: + parser.error("--suite and --task are required for --env libero") + + def get_env_spec() -> EnvSpec: """Return the LIBERO env identity + prompt bundle. diff --git a/robots/rebot_robstride/config.py b/robots/rebot_robstride/config.py index 2a2fab4a..5f993dc7 100644 --- a/robots/rebot_robstride/config.py +++ b/robots/rebot_robstride/config.py @@ -3,7 +3,7 @@ from __future__ import annotations import math -from dataclasses import dataclass, replace +from dataclasses import dataclass, fields, replace from pathlib import Path from typing import Any @@ -42,10 +42,18 @@ class RebotConfig: channel: str bitrate: int control_rate_hz: float + feedback_rate_hz: float read_timeout_ms: int settle_tolerance: float settle_timeout_s: float - shutdown_policy: str + settle_samples: int + settle_velocity_rad_s: float + startup_sample_interval_s: float + startup_velocity_limit_rad_s: float + max_tracking_error_rad: float + velocity_abort_multiplier: float + max_motion_duration_s: float + heartbeat_timeout_s: float joints: tuple[JointConfig, ...] gripper: GripperConfig @@ -65,10 +73,18 @@ def default_config() -> RebotConfig: channel="can0", bitrate=1_000_000, control_rate_hz=50.0, + feedback_rate_hz=10.0, read_timeout_ms=100, settle_tolerance=0.03, settle_timeout_s=2.0, - shutdown_policy="disable", + settle_samples=3, + settle_velocity_rad_s=0.05, + startup_sample_interval_s=0.1, + startup_velocity_limit_rad_s=0.1, + max_tracking_error_rad=0.35, + velocity_abort_multiplier=2.5, + max_motion_duration_s=60.0, + heartbeat_timeout_s=2.0, joints=joints, gripper=GripperConfig(), ) @@ -92,14 +108,27 @@ def load_config(path: str | Path | None = None) -> RebotConfig: if not isinstance(raw, dict): raise ValueError("reBot config must contain a YAML mapping") + allowed_root = {field.name for field in fields(RebotConfig)} + unknown_root = sorted(set(raw) - allowed_root) + if unknown_root: + raise ValueError(f"unknown reBot config fields: {', '.join(unknown_root)}") + scalar_fields = { "channel", "bitrate", "control_rate_hz", + "feedback_rate_hz", "read_timeout_ms", "settle_tolerance", "settle_timeout_s", - "shutdown_policy", + "settle_samples", + "settle_velocity_rad_s", + "startup_sample_interval_s", + "startup_velocity_limit_rad_s", + "max_tracking_error_rad", + "velocity_abort_multiplier", + "max_motion_duration_s", + "heartbeat_timeout_s", } updates = {key: raw[key] for key in scalar_fields if key in raw} @@ -139,23 +168,64 @@ def _joint_from_mapping(row: Any) -> JointConfig: def _validate(config: RebotConfig) -> RebotConfig: - if not config.channel: - raise ValueError("channel must be non-empty") - if config.bitrate <= 0: - raise ValueError("bitrate must be positive") - - if config.control_rate_hz <= 0: - raise ValueError("control_rate_hz must be positive") - if config.read_timeout_ms <= 0: - raise ValueError("read_timeout_ms must be positive") - if config.settle_tolerance <= 0 or config.settle_timeout_s <= 0: - raise ValueError("settle tolerances and timeouts must be positive") - if config.shutdown_policy not in {"disable", "hold"}: - raise ValueError("shutdown_policy must be 'disable' or 'hold'") + if not isinstance(config.channel, str) or not config.channel: + raise ValueError("channel must be a non-empty string") + if ( + isinstance(config.bitrate, bool) + or not isinstance(config.bitrate, int) + or config.bitrate <= 0 + ): + raise ValueError("bitrate must be a positive integer") + + if ( + isinstance(config.read_timeout_ms, bool) + or not isinstance(config.read_timeout_ms, int) + or not 1 <= config.read_timeout_ms <= 1000 + ): + raise ValueError("read_timeout_ms must be an integer in [1, 1000]") + + finite_positive = { + "control_rate_hz": config.control_rate_hz, + "feedback_rate_hz": config.feedback_rate_hz, + "settle_tolerance": config.settle_tolerance, + "settle_timeout_s": config.settle_timeout_s, + "settle_velocity_rad_s": config.settle_velocity_rad_s, + "startup_sample_interval_s": config.startup_sample_interval_s, + "startup_velocity_limit_rad_s": config.startup_velocity_limit_rad_s, + "max_tracking_error_rad": config.max_tracking_error_rad, + "velocity_abort_multiplier": config.velocity_abort_multiplier, + "max_motion_duration_s": config.max_motion_duration_s, + "heartbeat_timeout_s": config.heartbeat_timeout_s, + } + for name, value in finite_positive.items(): + if ( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not math.isfinite(value) + or value <= 0 + ): + raise ValueError(f"{name} must be finite and positive") + if config.control_rate_hz > 200: + raise ValueError("control_rate_hz must not exceed 200 Hz") + if config.feedback_rate_hz > config.control_rate_hz: + raise ValueError("feedback_rate_hz must not exceed control_rate_hz") + if ( + isinstance(config.settle_samples, bool) + or not isinstance(config.settle_samples, int) + or config.settle_samples < 1 + ): + raise ValueError("settle_samples must be a positive integer") + if config.max_motion_duration_s >= 120: + raise ValueError("max_motion_duration_s must remain below the RPC timeout") if len(config.joints) != 6: raise ValueError("exactly six arm joints are required") motor_ids = [joint.motor_id for joint in config.joints] + [config.gripper.motor_id] + if any( + isinstance(motor_id, bool) or not isinstance(motor_id, int) + for motor_id in motor_ids + ): + raise ValueError("motor IDs must be integers") if len(set(motor_ids)) != len(motor_ids): raise ValueError("motor IDs must be unique") if any(not 1 <= motor_id <= 0xFF for motor_id in motor_ids): @@ -167,7 +237,12 @@ def _validate(config: RebotConfig) -> RebotConfig: for joint in config.joints: values = (joint.lower, joint.upper, joint.kp, joint.kd, joint.max_velocity) - if not all(math.isfinite(value) for value in values): + if not all( + isinstance(value, (int, float)) + and not isinstance(value, bool) + and math.isfinite(value) + for value in values + ): raise ValueError(f"{joint.name} contains a non-finite value") if joint.lower >= joint.upper: raise ValueError(f"{joint.name} lower limit must be below upper limit") @@ -182,13 +257,23 @@ def _validate(config: RebotConfig) -> RebotConfig: "gripper open_position and closed_position must both be set or both be null" ) gripper_values = (gripper.kp, gripper.kd, gripper.max_velocity) - if not all(math.isfinite(value) for value in gripper_values): + if not all( + isinstance(value, (int, float)) + and not isinstance(value, bool) + and math.isfinite(value) + for value in gripper_values + ): raise ValueError("gripper contains a non-finite value") if gripper.kp < 0 or gripper.kd < 0 or gripper.max_velocity <= 0: raise ValueError("gripper gains must be non-negative and max_velocity positive") if gripper.open_position is not None: endpoints = (gripper.open_position, gripper.closed_position) - if not all(value is not None and math.isfinite(value) for value in endpoints): + if not all( + isinstance(value, (int, float)) + and not isinstance(value, bool) + and math.isfinite(value) + for value in endpoints + ): raise ValueError("gripper endpoints must be finite") if gripper.open_position == gripper.closed_position: raise ValueError("gripper endpoints must differ") diff --git a/robots/rebot_robstride/config/rebot_robstride.example.yaml b/robots/rebot_robstride/config/rebot_robstride.example.yaml index 43511d26..38a945f0 100644 --- a/robots/rebot_robstride/config/rebot_robstride.example.yaml +++ b/robots/rebot_robstride/config/rebot_robstride.example.yaml @@ -4,10 +4,18 @@ channel: can0 bitrate: 1000000 control_rate_hz: 50 +feedback_rate_hz: 10 read_timeout_ms: 100 settle_tolerance: 0.03 settle_timeout_s: 2.0 -shutdown_policy: disable +settle_samples: 3 +settle_velocity_rad_s: 0.05 +startup_sample_interval_s: 0.1 +startup_velocity_limit_rad_s: 0.1 +max_tracking_error_rad: 0.35 +velocity_abort_multiplier: 2.5 +max_motion_duration_s: 60 +heartbeat_timeout_s: 2.0 # Limits are raw motor coordinates. Keep max_velocity <= 1.0 rad/s. joints: diff --git a/robots/rebot_robstride/driver.py b/robots/rebot_robstride/driver.py index 7b82b1b2..01937219 100644 --- a/robots/rebot_robstride/driver.py +++ b/robots/rebot_robstride/driver.py @@ -1,4 +1,4 @@ -"""Safe motorbridge driver for the seven-motor reBot DevArm RobStride arm.""" +"""Safety-focused motorbridge driver for the seven-motor reBot DevArm.""" from __future__ import annotations @@ -6,14 +6,18 @@ import threading import time from collections.abc import Callable, Sequence -from typing import Any +from typing import Any, NoReturn -from robots.rebot_robstride.config import JointConfig, RebotConfig +from robots.rebot_robstride.config import GripperConfig, JointConfig, RebotConfig MECH_POS = 0x7019 -MECH_VEL = 0x701A ROBSTRIDE_HOST_ID = 0xFD MIT_MODE = 1 +MIN_JERK_PEAK_VELOCITY = 1.875 + + +class MotionCancelled(RuntimeError): + """Raised inside a trajectory after a preemptive stop request.""" def _default_controller_factory(channel: str): @@ -45,30 +49,50 @@ def __init__( self._controller_factory = controller_factory or _default_controller_factory self._clock = clock self._sleep = sleep - self._lock = threading.RLock() + + # Lock order is motion -> I/O -> state. The emergency-stop path never + # waits for the motion lock and sets cancellation before waiting for I/O. + self._motion_lock = threading.Lock() + self._io_lock = threading.RLock() + self._state_lock = threading.RLock() + self._cancel_event = threading.Event() + self._controller = None self._motors: dict[int, Any] = {} self._enabled = False + self._gripper_enabled = False self._stopped = False self._last_targets: list[float] | None = None + self._previous_positions: dict[int, float] | None = None + self._previous_sample_time: float | None = None + self._last_heartbeat = self._clock() @property def connected(self) -> bool: - return self._controller is not None + with self._state_lock: + return self._controller is not None @property def enabled(self) -> bool: - return self._enabled + with self._state_lock: + return self._enabled @property def stopped(self) -> bool: - return self._stopped + with self._state_lock: + return self._stopped def connect(self) -> dict[str, Any]: """Open CAN, register all motors, and take a passive state snapshot.""" - with self._lock: - if self.connected: + with self._motion_lock: + with self._state_lock: + if self._controller is not None: + already_connected = True + else: + already_connected = False + if already_connected: return self.state() + controller = self._controller_factory(self.config.channel) try: motor_configs = [*self.config.joints, self.config.gripper] @@ -80,8 +104,9 @@ def connect(self) -> dict[str, Any]: ) for motor in motor_configs } - self._controller = controller - self._motors = motors + with self._state_lock: + self._controller = controller + self._motors = motors snapshot = self.state() self._last_targets = list(snapshot["joint_positions"]) return snapshot @@ -89,70 +114,103 @@ def connect(self) -> dict[str, Any]: try: controller.close() finally: - self._controller = None - self._motors = {} + with self._state_lock: + self._controller = None + self._motors = {} raise def state(self) -> dict[str, Any]: - """Return a fresh all-motor snapshot using RobStride parameter reads.""" - with self._lock: - self._require_connected() - positions: list[float] = [] - velocities: list[float] = [] - for joint in self.config.joints: - position, velocity = self._read_motor(joint.motor_id) - positions.append(position) - velocities.append(velocity) - gripper_position, gripper_velocity = self._read_motor( - self.config.gripper.motor_id - ) - return { - "connected": True, - "enabled": self._enabled, - "stopped": self._stopped, - "channel": self.config.channel, - "joint_names": [joint.name for joint in self.config.joints], - "joint_positions": positions, - "joint_velocities": velocities, - "gripper_position": gripper_position, - "gripper_velocity": gripper_velocity, - "timestamp": self._clock(), - } + """Return fresh positions, estimated velocities, and fault reports.""" + snapshot = self._sample_feedback() + with self._state_lock: + enabled = self._enabled + gripper_enabled = self._gripper_enabled + stopped = self._stopped + joint_ids = [joint.motor_id for joint in self.config.joints] + gripper_id = self.config.gripper.motor_id + return { + "connected": True, + "enabled": enabled, + "gripper_enabled": gripper_enabled, + "stopped": stopped, + "channel": self.config.channel, + "joint_names": [joint.name for joint in self.config.joints], + "joint_positions": [ + snapshot["positions"][motor_id] for motor_id in joint_ids + ], + "joint_velocities": [ + snapshot["velocities"][motor_id] for motor_id in joint_ids + ], + "gripper_position": snapshot["positions"][gripper_id], + "gripper_velocity": snapshot["velocities"][gripper_id], + "faults": snapshot["faults"], + "timestamp": snapshot["timestamp"], + } def enable(self) -> dict[str, Any]: - """Clear faults, select MIT mode, and hold the observed pose.""" - with self._lock: + """Validate startup state, enable arm joints, and hold observed pose.""" + with self._motion_lock: self._require_connected() - controller = self._controller - assert controller is not None - snapshot = self.state() - hold_targets = [ - *snapshot["joint_positions"], - snapshot["gripper_position"], - ] - motor_configs = [*self.config.joints, self.config.gripper] - for motor_config in motor_configs: - motor = self._motors[motor_config.motor_id] - motor.clear_error() - motor.ensure_mode(MIT_MODE, timeout_ms=1000) + with self._state_lock: + if self._stopped: + raise RuntimeError( + "arm is stopped; call reset_stop before enabling" + ) + self._cancel_event.clear() + first = self._sample_feedback() + self._validate_startup_positions(first) + self._sleep(self.config.startup_sample_interval_s) + second = self._sample_feedback() + self._validate_startup_positions(second) + self._validate_startup_velocity(second) + + arm_configs = list(self.config.joints) try: - controller.enable_all() + for motor_config in arm_configs: + with self._io_lock: + self._raise_if_cancelled() + motor = self._motors[motor_config.motor_id] + motor.clear_error() + with self._io_lock: + self._raise_if_cancelled() + self._assert_no_faults( + self._read_faults_locked(), + {joint.motor_id for joint in arm_configs}, + context="startup", + ) + for motor_config in arm_configs: + with self._io_lock: + self._raise_if_cancelled() + self._motors[motor_config.motor_id].ensure_mode( + MIT_MODE, timeout_ms=1000 + ) + for motor_config in arm_configs: + with self._io_lock: + self._raise_if_cancelled() + self._motors[motor_config.motor_id].enable() + for motor_config in arm_configs: + with self._io_lock: + self._raise_if_cancelled() + self._send_mit_locked( + motor_config, + second["positions"][motor_config.motor_id], + ) + except Exception as exc: + self._fail_closed(exc) + + with self._state_lock: self._enabled = True - for motor_config, target in zip(motor_configs, hold_targets): - self._send_mit(motor_config, target) - except Exception: - self._stopped = True - try: - controller.disable_all() - finally: - self._enabled = False - raise - self._stopped = False - self._last_targets = list(snapshot["joint_positions"]) + self._gripper_enabled = False + self._stopped = False + self._cancel_event.clear() + self._last_heartbeat = self._clock() + self._last_targets = [ + second["positions"][joint.motor_id] for joint in arm_configs + ] return { "enabled": True, - "hold_positions": list(snapshot["joint_positions"]), - "gripper_hold_position": snapshot["gripper_position"], + "gripper_enabled": False, + "hold_positions": list(self._last_targets), } def move_joints( @@ -161,8 +219,8 @@ def move_joints( *, duration_s: float = 2.0, ) -> dict[str, Any]: - """Execute a minimum-jerk joint trajectory and verify final feedback.""" - with self._lock: + """Execute a monitored minimum-jerk trajectory and verify settlement.""" + with self._motion_lock: self._require_motion_ready() target = [float(value) for value in positions] if len(target) != len(self.config.joints): @@ -170,45 +228,76 @@ def move_joints( if not all(math.isfinite(value) for value in target): raise ValueError("joint targets must be finite") self._validate_joint_limits(target) - if not math.isfinite(duration_s) or duration_s <= 0: - raise ValueError("duration_s must be positive and finite") + requested_duration = self._validate_duration(duration_s) - start_state = self.state() - start = list(start_state["joint_positions"]) - minimum_duration = max( + self._cancel_event.clear() + try: + start_state = self._sample_feedback() + start = [ + start_state["positions"][joint.motor_id] + for joint in self.config.joints + ] + self._validate_motion_feedback(start_state, start) + except Exception as exc: + self._fail_closed(exc) + minimum_duration = MIN_JERK_PEAK_VELOCITY * max( abs(goal - initial) / joint.max_velocity for initial, goal, joint in zip(start, target, self.config.joints) ) - actual_duration = max(float(duration_s), minimum_duration) + actual_duration = max(requested_duration, minimum_duration) + self._check_actual_duration(actual_duration) steps = max(2, math.ceil(actual_duration * self.config.control_rate_hz)) interval = actual_duration / steps + feedback_stride = max( + 1, + round(self.config.control_rate_hz / self.config.feedback_rate_hz), + ) start_time = self._clock() + try: for index in range(1, steps + 1): + self._raise_if_cancelled() scale = _min_jerk(index / steps) waypoint = [ initial + (goal - initial) * scale for initial, goal in zip(start, target) ] - for joint, position in zip(self.config.joints, waypoint): - self._send_mit(joint, position) - self._last_targets = waypoint + with self._io_lock: + for joint, position in zip(self.config.joints, waypoint): + self._send_mit_locked(joint, position) + with self._state_lock: + self._last_targets = waypoint + remaining = start_time + index * interval - self._clock() - if remaining > 0: - self._sleep(remaining) + if remaining > 0 and self._sleep_interruptible(remaining): + raise MotionCancelled("joint motion cancelled by stop request") + if index % feedback_stride == 0 or index == steps: + self._validate_motion_feedback( + self._sample_feedback(), waypoint + ) - final = self._wait_for_target(target) - except Exception: - self._stopped = True + settled = self._wait_for_joint_target(target) + except MotionCancelled: raise - errors = [abs(observed - goal) for observed, goal in zip(final, target)] + except Exception as exc: + self._fail_closed(exc) + + errors = [ + abs(observed - goal) + for observed, goal in zip(settled["positions"], target) + ] max_error = max(errors) + reached = bool(settled["reached"]) + if not reached: + self._disable_after_timeout("joint settlement timed out") return { "target_positions": target, - "final_positions": final, + "final_positions": settled["positions"], + "final_velocities": settled["velocities"], "max_error": max_error, - "reached": max_error <= self.config.settle_tolerance, - "requested_duration_s": float(duration_s), + "reached": reached, + "enabled": self.enabled, + "requested_duration_s": requested_duration, "actual_duration_s": actual_duration, } @@ -218,132 +307,466 @@ def set_gripper( *, duration_s: float = 1.0, ) -> dict[str, Any]: - """Move the calibrated gripper; 0.0=open and 1.0=closed.""" - with self._lock: + """Enable and move the calibrated gripper; 0.0=open and 1.0=closed.""" + with self._motion_lock: self._require_motion_ready() normalized = float(position) if not math.isfinite(normalized) or not 0.0 <= normalized <= 1.0: raise ValueError("gripper position must be in [0.0, 1.0]") - if not math.isfinite(duration_s) or duration_s <= 0: - raise ValueError("duration_s must be positive and finite") + requested_duration = self._validate_duration(duration_s) gripper = self.config.gripper if gripper.open_position is None or gripper.closed_position is None: raise RuntimeError( "gripper is not calibrated; configure open_position and closed_position" ) + + self._cancel_event.clear() + try: + snapshot = self._sample_feedback() + except Exception as exc: + self._fail_closed(exc) + start = snapshot["positions"][gripper.motor_id] + travel_lower = min(gripper.open_position, gripper.closed_position) + travel_upper = max(gripper.open_position, gripper.closed_position) + if not travel_lower <= start <= travel_upper: + raise RuntimeError( + f"gripper feedback {start:.6f} is outside calibrated travel " + f"[{travel_lower:.6f}, {travel_upper:.6f}]" + ) target = gripper.open_position + normalized * ( gripper.closed_position - gripper.open_position ) - start, _ = self._read_motor(gripper.motor_id) - minimum_duration = abs(target - start) / gripper.max_velocity - actual_duration = max(float(duration_s), minimum_duration) + self._ensure_gripper_enabled(gripper, start) + + minimum_duration = ( + MIN_JERK_PEAK_VELOCITY * abs(target - start) / gripper.max_velocity + ) + actual_duration = max(requested_duration, minimum_duration) + self._check_actual_duration(actual_duration) steps = max(2, math.ceil(actual_duration * self.config.control_rate_hz)) interval = actual_duration / steps + feedback_stride = max( + 1, + round(self.config.control_rate_hz / self.config.feedback_rate_hz), + ) start_time = self._clock() + try: for index in range(1, steps + 1): - scale = _min_jerk(index / steps) - waypoint = start + (target - start) * scale - self._send_mit(gripper, waypoint) + self._raise_if_cancelled() + waypoint = start + (target - start) * _min_jerk(index / steps) + with self._io_lock: + self._send_mit_locked(gripper, waypoint) remaining = start_time + index * interval - self._clock() - if remaining > 0: - self._sleep(remaining) - final, _ = self._read_motor(gripper.motor_id) - except Exception: - self._stopped = True + if remaining > 0 and self._sleep_interruptible(remaining): + raise MotionCancelled( + "gripper motion cancelled by stop request" + ) + if index % feedback_stride == 0 or index == steps: + feedback = self._sample_feedback() + self._validate_gripper_feedback(feedback, waypoint) + + settled = self._wait_for_gripper_target(target) + except MotionCancelled: raise + except Exception as exc: + self._fail_closed(exc) + + error = abs(settled["position"] - target) + reached = bool(settled["reached"]) + if not reached: + self._disable_after_timeout("gripper settlement timed out") return { "normalized_position": normalized, "target_position": target, - "final_position": final, + "final_position": settled["position"], + "final_velocity": settled["velocity"], + "max_error": error, + "reached": reached, + "enabled": self.enabled, "actual_duration_s": actual_duration, } def stop_motion(self) -> dict[str, Any]: - """Latch a torque-holding software stop and reject new trajectories.""" - with self._lock: - self._require_connected() + """Preempt motion, latch a software stop, and hold fresh arm feedback.""" + self._cancel_event.set() + with self._state_lock: + self._require_connected_locked() self._stopped = True - if self._enabled: - snapshot = self.state() - for joint, position in zip( - self.config.joints, snapshot["joint_positions"] - ): - self._send_mit(joint, position) - self._last_targets = list(snapshot["joint_positions"]) - return {"enabled": self._enabled, "stopped": True} + enabled = self._enabled + if enabled: + try: + snapshot = self._sample_feedback() + self._validate_startup_positions(snapshot) + self._assert_no_faults( + snapshot["faults"], + {joint.motor_id for joint in self.config.joints}, + context="soft stop", + ) + with self._io_lock: + for joint in self.config.joints: + self._send_mit_locked( + joint, snapshot["positions"][joint.motor_id] + ) + with self._state_lock: + self._last_targets = [ + snapshot["positions"][joint.motor_id] + for joint in self.config.joints + ] + except Exception as exc: + self._fail_closed(exc) + return {"enabled": self.enabled, "stopped": True} def reset_stop(self) -> dict[str, Any]: """Clear only the software-stop latch; it never enables motors.""" - with self._lock: - self._require_connected() + with self._state_lock: + self._require_connected_locked() self._stopped = False + self._cancel_event.clear() return {"enabled": self._enabled, "stopped": False} def emergency_stop(self) -> dict[str, Any]: - """Disable every motor immediately; the unsupported arm may fall.""" - with self._lock: - self._require_connected() - self._controller.disable_all() - self._enabled = False + """Preempt motion and disable every motor without waiting for trajectory locks.""" + self._cancel_event.set() + with self._state_lock: + self._require_connected_locked() self._stopped = True - return {"enabled": False, "stopped": True} + self._disable_all() + return {"enabled": False, "stopped": True} + + def heartbeat(self) -> dict[str, Any]: + """Refresh the agent-process deadman without changing motor state.""" + with self._state_lock: + self._require_connected_locked() + self._last_heartbeat = self._clock() + return {"ok": True, "enabled": self._enabled} + + def enforce_heartbeat_deadman(self) -> bool: + """Disable enabled motors after the agent heartbeat expires.""" + with self._state_lock: + expired = ( + self._enabled or self._gripper_enabled + ) and self._clock() - self._last_heartbeat > self.config.heartbeat_timeout_s + if not expired: + return False + self.emergency_stop() + return True def close(self) -> None: - """Apply the configured shutdown policy and release the CAN controller.""" - with self._lock: - if self._controller is None: + """Cancel motion, disable enabled motors, and release the controller.""" + self._cancel_event.set() + with self._motion_lock: + with self._state_lock: + controller = self._controller + should_disable = self._enabled or self._gripper_enabled + if controller is None: return try: - if self.config.shutdown_policy == "disable" and self._enabled: - self._controller.disable_all() - self._enabled = False - elif self._enabled and self._last_targets is not None: - for joint, target in zip(self.config.joints, self._last_targets): - self._send_mit(joint, target) + if should_disable: + self._disable_all() finally: - self._controller.close() - self._controller = None - self._motors = {} - self._enabled = False - self._stopped = True - - def _read_motor(self, motor_id: int) -> tuple[float, float]: - motor = self._motors[motor_id] + with self._io_lock: + controller.close() + with self._state_lock: + self._controller = None + self._motors = {} + self._enabled = False + self._gripper_enabled = False + self._stopped = True + self._previous_positions = None + self._previous_sample_time = None + + def _sample_feedback(self) -> dict[str, Any]: + self._require_connected() + with self._io_lock: + positions = { + motor_id: self._read_position_locked(motor_id) + for motor_id in self._motors + } + faults = self._read_faults_locked() + timestamp = self._clock() + with self._state_lock: + previous_positions = self._previous_positions + previous_time = self._previous_sample_time + if ( + previous_positions is None + or previous_time is None + or timestamp <= previous_time + ): + velocities = dict.fromkeys(positions, 0.0) + else: + elapsed = timestamp - previous_time + velocities = { + motor_id: (position - previous_positions[motor_id]) / elapsed + for motor_id, position in positions.items() + } + self._previous_positions = dict(positions) + self._previous_sample_time = timestamp + return { + "positions": positions, + "velocities": velocities, + "faults": faults, + "timestamp": timestamp, + } + + def _read_position_locked(self, motor_id: int) -> float: position = float( - motor.robstride_get_param_f32( + self._motors[motor_id].robstride_get_param_f32( MECH_POS, timeout_ms=self.config.read_timeout_ms ) ) - velocity = float( - motor.robstride_get_param_f32( - MECH_VEL, timeout_ms=self.config.read_timeout_ms + if not math.isfinite(position): + raise RuntimeError( + f"motor {motor_id} returned non-finite position feedback" ) - ) - if not math.isfinite(position) or not math.isfinite(velocity): - raise RuntimeError(f"motor {motor_id} returned non-finite feedback") - return position, velocity + return position + + def _read_faults_locked(self) -> dict[int, dict[str, int]]: + reports: dict[int, dict[str, int]] = {} + for motor_id, motor in self._motors.items(): + fault_raw, warning_raw = motor.robstride_get_fault_report() + reports[motor_id] = { + "fault_raw": int(fault_raw), + "warning_raw": int(warning_raw), + } + return reports - def _send_mit(self, config: JointConfig | Any, position: float) -> None: + def _send_mit_locked( + self, config: JointConfig | GripperConfig, position: float + ) -> None: self._motors[config.motor_id].send_mit( float(position), 0.0, float(config.kp), float(config.kd), 0.0 ) - def _wait_for_target(self, target: list[float]) -> list[float]: + def _validate_startup_positions(self, snapshot: dict[str, Any]) -> None: + positions = [ + snapshot["positions"][joint.motor_id] for joint in self.config.joints + ] + self._validate_joint_limits(positions) + + def _validate_startup_velocity(self, snapshot: dict[str, Any]) -> None: + for joint in self.config.joints: + velocity = abs(snapshot["velocities"][joint.motor_id]) + if velocity > self.config.startup_velocity_limit_rad_s: + raise RuntimeError( + f"{joint.name} startup velocity {velocity:.6f} rad/s exceeds " + f"{self.config.startup_velocity_limit_rad_s:.6f} rad/s" + ) + + def _validate_motion_feedback( + self, snapshot: dict[str, Any], waypoint: Sequence[float] + ) -> None: + arm_ids = {joint.motor_id for joint in self.config.joints} + self._assert_no_faults(snapshot["faults"], arm_ids, context="motion") + for joint, expected in zip(self.config.joints, waypoint): + observed = snapshot["positions"][joint.motor_id] + error = abs(observed - expected) + if error > self.config.max_tracking_error_rad: + raise RuntimeError( + f"{joint.name} tracking error {error:.6f} rad exceeds " + f"{self.config.max_tracking_error_rad:.6f} rad" + ) + velocity = abs(snapshot["velocities"][joint.motor_id]) + velocity_limit = joint.max_velocity * self.config.velocity_abort_multiplier + if velocity > velocity_limit: + raise RuntimeError( + f"{joint.name} feedback velocity {velocity:.6f} rad/s exceeds " + f"abort limit {velocity_limit:.6f} rad/s" + ) + + def _validate_gripper_feedback( + self, snapshot: dict[str, Any], waypoint: float + ) -> None: + gripper = self.config.gripper + self._assert_no_faults( + snapshot["faults"], {gripper.motor_id}, context="gripper motion" + ) + error = abs(snapshot["positions"][gripper.motor_id] - waypoint) + if error > self.config.max_tracking_error_rad: + raise RuntimeError( + f"gripper tracking error {error:.6f} rad exceeds " + f"{self.config.max_tracking_error_rad:.6f} rad" + ) + velocity = abs(snapshot["velocities"][gripper.motor_id]) + limit = gripper.max_velocity * self.config.velocity_abort_multiplier + if velocity > limit: + raise RuntimeError( + f"gripper feedback velocity {velocity:.6f} rad/s exceeds " + f"abort limit {limit:.6f} rad/s" + ) + + @staticmethod + def _assert_no_faults( + reports: dict[int, dict[str, int]], motor_ids: set[int], *, context: str + ) -> None: + active = { + motor_id: reports[motor_id] + for motor_id in sorted(motor_ids) + if reports[motor_id]["fault_raw"] or reports[motor_id]["warning_raw"] + } + if active: + raise RuntimeError(f"RobStride fault during {context}: {active}") + + def _ensure_gripper_enabled(self, gripper: GripperConfig, hold: float) -> None: + with self._state_lock: + if self._gripper_enabled: + return + try: + motor = self._motors[gripper.motor_id] + with self._io_lock: + self._raise_if_cancelled() + motor.clear_error() + with self._io_lock: + self._raise_if_cancelled() + self._assert_no_faults( + self._read_faults_locked(), + {gripper.motor_id}, + context="gripper startup", + ) + with self._io_lock: + self._raise_if_cancelled() + motor.ensure_mode(MIT_MODE, timeout_ms=1000) + with self._io_lock: + self._raise_if_cancelled() + motor.enable() + with self._io_lock: + self._raise_if_cancelled() + self._send_mit_locked(gripper, hold) + with self._state_lock: + self._gripper_enabled = True + except Exception as exc: + self._fail_closed(exc) + + def _wait_for_joint_target(self, target: list[float]) -> dict[str, Any]: deadline = self._clock() + self.config.settle_timeout_s - final: list[float] = [] + consecutive = 0 + final_positions = target + final_velocities = [math.inf] * len(target) while True: - final = list(self.state()["joint_positions"]) - if ( - max(abs(value - goal) for value, goal in zip(final, target)) + self._raise_if_cancelled() + snapshot = self._sample_feedback() + self._validate_motion_feedback(snapshot, target) + final_positions = [ + snapshot["positions"][joint.motor_id] for joint in self.config.joints + ] + final_velocities = [ + snapshot["velocities"][joint.motor_id] for joint in self.config.joints + ] + positions_ok = ( + max(abs(value - goal) for value, goal in zip(final_positions, target)) <= self.config.settle_tolerance - ): - return final + ) + velocities_ok = max(abs(value) for value in final_velocities) <= ( + self.config.settle_velocity_rad_s + ) + consecutive = consecutive + 1 if positions_ok and velocities_ok else 0 + if consecutive >= self.config.settle_samples: + return { + "positions": final_positions, + "velocities": final_velocities, + "reached": True, + } + if self._clock() >= deadline: + return { + "positions": final_positions, + "velocities": final_velocities, + "reached": False, + } + if self._sleep_interruptible(1.0 / self.config.feedback_rate_hz): + raise MotionCancelled("joint settlement cancelled by stop request") + + def _wait_for_gripper_target(self, target: float) -> dict[str, Any]: + deadline = self._clock() + self.config.settle_timeout_s + consecutive = 0 + motor_id = self.config.gripper.motor_id + final_position = target + final_velocity = math.inf + while True: + self._raise_if_cancelled() + snapshot = self._sample_feedback() + self._validate_gripper_feedback(snapshot, target) + final_position = snapshot["positions"][motor_id] + final_velocity = snapshot["velocities"][motor_id] + settled = ( + abs(final_position - target) <= self.config.settle_tolerance + and abs(final_velocity) <= self.config.settle_velocity_rad_s + ) + consecutive = consecutive + 1 if settled else 0 + if consecutive >= self.config.settle_samples: + return { + "position": final_position, + "velocity": final_velocity, + "reached": True, + } if self._clock() >= deadline: - return final - self._sleep(0.05) + return { + "position": final_position, + "velocity": final_velocity, + "reached": False, + } + if self._sleep_interruptible(1.0 / self.config.feedback_rate_hz): + raise MotionCancelled("gripper settlement cancelled by stop request") + + def _sleep_interruptible(self, duration_s: float) -> bool: + deadline = self._clock() + max(0.0, duration_s) + while True: + if self._cancel_event.is_set(): + return True + remaining = deadline - self._clock() + if remaining <= 0: + return False + self._sleep(min(remaining, 0.01)) - def _validate_joint_limits(self, target: list[float]) -> None: + def _raise_if_cancelled(self) -> None: + if self._cancel_event.is_set(): + raise MotionCancelled("motion cancelled by stop request") + + def _validate_duration(self, duration_s: float) -> float: + duration = float(duration_s) + if not math.isfinite(duration) or duration <= 0: + raise ValueError("duration_s must be positive and finite") + if duration > self.config.max_motion_duration_s: + raise ValueError( + f"duration_s must not exceed {self.config.max_motion_duration_s:.3f}" + ) + return duration + + def _check_actual_duration(self, duration_s: float) -> None: + if duration_s > self.config.max_motion_duration_s: + raise ValueError( + "velocity-limited trajectory duration exceeds max_motion_duration_s" + ) + + def _disable_after_timeout(self, reason: str) -> None: + try: + self._disable_all() + except Exception as exc: + raise RuntimeError(f"{reason}; disable_all also failed: {exc}") from exc + + def _fail_closed(self, error: Exception) -> NoReturn: + self._cancel_event.set() + with self._state_lock: + self._stopped = True + try: + self._disable_all() + except Exception as disable_error: + raise RuntimeError( + f"{error}; fail-closed disable_all also failed: {disable_error}" + ) from error + raise error + + def _disable_all(self) -> None: + with self._io_lock: + controller = self._controller + if controller is None: + return + controller.disable_all() + with self._state_lock: + self._enabled = False + self._gripper_enabled = False + self._stopped = True + + def _validate_joint_limits(self, target: Sequence[float]) -> None: for joint, value in zip(self.config.joints, target): if not joint.lower <= value <= joint.upper: raise ValueError( @@ -352,12 +775,17 @@ def _validate_joint_limits(self, target: list[float]) -> None: ) def _require_connected(self) -> None: + with self._state_lock: + self._require_connected_locked() + + def _require_connected_locked(self) -> None: if self._controller is None: raise RuntimeError("arm is not connected") def _require_motion_ready(self) -> None: - self._require_connected() - if not self._enabled: - raise RuntimeError("arm is not enabled") - if self._stopped: - raise RuntimeError("arm is stopped; call reset_stop before moving") + with self._state_lock: + self._require_connected_locked() + if not self._enabled: + raise RuntimeError("arm is not enabled") + if self._stopped: + raise RuntimeError("arm is stopped; call reset_stop before moving") diff --git a/robots/rebot_robstride/env_client.py b/robots/rebot_robstride/env_client.py index a03b4975..8e20b3dc 100644 --- a/robots/rebot_robstride/env_client.py +++ b/robots/rebot_robstride/env_client.py @@ -7,7 +7,7 @@ from rpent.utils.rpc import RpcClient _DEFAULT_TIMEOUT_S = 10.0 -_MOTION_TIMEOUT_S = 120.0 +_MOTION_TIMEOUT_S = 75.0 class RebotRobstrideEnvClient: @@ -48,3 +48,6 @@ def reset_stop(self) -> dict[str, Any]: def emergency_stop(self) -> dict[str, Any]: return self._client.call("robot.emergency_stop", timeout_s=_DEFAULT_TIMEOUT_S) + + def heartbeat(self) -> dict[str, Any]: + return self._client.call("robot.heartbeat", timeout_s=1.0) diff --git a/robots/rebot_robstride/env_server.py b/robots/rebot_robstride/env_server.py index 0bccefc1..f18654e4 100644 --- a/robots/rebot_robstride/env_server.py +++ b/robots/rebot_robstride/env_server.py @@ -4,6 +4,7 @@ from __future__ import annotations import argparse +import ipaddress import json import re import signal @@ -27,10 +28,12 @@ def make_dispatch(driver: Any, shutdown_event: threading.Event): "robot.stop_motion": driver.stop_motion, "robot.reset_stop": driver.reset_stop, "robot.emergency_stop": driver.emergency_stop, + "robot.heartbeat": driver.heartbeat, } def dispatch(method: str, args: tuple, kwargs: dict): if method == "shutdown": + driver.emergency_stop() shutdown_event.set() return {"ok": True} handler = handlers.get(method) @@ -41,6 +44,19 @@ def dispatch(method: str, args: tuple, kwargs: dict): return dispatch +def validate_loopback_host(host: str) -> None: + """Reject network-exposed pickle RPC for the physical-control backend.""" + normalized = host.strip().lower() + if normalized == "localhost": + return + try: + address = ipaddress.ip_address(normalized) + except ValueError as exc: + raise ValueError("reBot RPC host must be a loopback address") from exc + if not address.is_loopback: + raise ValueError("reBot RPC host must be a loopback address") + + def validate_socketcan(channel: str, bitrate: int) -> None: """Fail with an actionable message unless SocketCAN is up at the expected rate.""" result = subprocess.run( @@ -80,6 +96,7 @@ def _build_parser() -> argparse.ArgumentParser: def main() -> int: args = _build_parser().parse_args() + validate_loopback_host(args.host) config = load_config(args.config) validate_socketcan(config.channel, config.bitrate) @@ -89,11 +106,47 @@ def main() -> int: try: initial_state = driver.connect() server = SocketRpcServer( - (args.host, args.port), make_dispatch(driver, shutdown_event) + (args.host, args.port), + make_dispatch(driver, shutdown_event), + priority_methods={ + "robot.stop_motion", + "robot.emergency_stop", + "robot.heartbeat", + "shutdown", + }, ) host, port = server.server_address threading.Thread(target=server.serve_forever, daemon=True).start() + def enforce_deadman() -> None: + interval_s = min(max(config.heartbeat_timeout_s / 4.0, 0.05), 0.5) + while not shutdown_event.wait(interval_s): + try: + if driver.enforce_heartbeat_deadman(): + print( + json.dumps( + { + "event": "heartbeat_deadman", + "action": "emergency_stop", + } + ), + flush=True, + ) + except Exception as exc: + print( + json.dumps( + { + "event": "heartbeat_deadman_error", + "error": str(exc), + } + ), + flush=True, + ) + shutdown_event.set() + return + + threading.Thread(target=enforce_deadman, daemon=True).start() + def request_shutdown(_signum, _frame) -> None: shutdown_event.set() diff --git a/robots/rebot_robstride/prompts/system.py b/robots/rebot_robstride/prompts/system.py index 0ca797e8..c3e9ada1 100644 --- a/robots/rebot_robstride/prompts/system.py +++ b/robots/rebot_robstride/prompts/system.py @@ -7,12 +7,13 @@ Safety rules: - Always call get_robot_state before enable_arm and before planning motion. -- Motors start disabled. Never claim motion happened until a tool returns reached=true - with final_positions read back from the hardware. +- Motors start disabled. Never claim arm or gripper motion happened until a tool returns + reached=true with final hardware feedback. - Use small, deliberate joint-space moves. Respect the configured raw-motor joint limits. - Never retry a rejected or failed motion blindly; read state and explain the failure. - stop_motion holds the observed pose and latches a software stop. - emergency_stop disables torque. An unsupported arm can fall under gravity afterward. +- A heartbeat protects agent-process loss, but it cannot replace the physical emergency stop. - Gripper tools refuse motion until open and closed endpoints are calibrated. - Call finish when the instruction is complete, failed, or unsafe to continue. """ diff --git a/robots/rebot_robstride/runtime.py b/robots/rebot_robstride/runtime.py index 1bd87ef3..e73ae94d 100644 --- a/robots/rebot_robstride/runtime.py +++ b/robots/rebot_robstride/runtime.py @@ -4,9 +4,11 @@ import subprocess import sys +import threading from pathlib import Path from typing import Any +from robots.rebot_robstride.config import load_config from robots.rebot_robstride.env_client import RebotRobstrideEnvClient from robots.rebot_robstride.toolkit import RebotRobstrideToolkit from rpent.envs.process import start_socket_server_process, stop_socket_server_process @@ -29,6 +31,9 @@ def __init__( self.output_dir = Path(output_dir) self.dashboard = dashboard self._process: subprocess.Popen | None = None + self._client: RebotRobstrideEnvClient | None = None + self._heartbeat_stop = threading.Event() + self._heartbeat_thread: threading.Thread | None = None def start(self) -> RebotRobstrideToolkit: if not self.args.no_driver: @@ -57,9 +62,36 @@ def start(self) -> RebotRobstrideToolkit: ) client = RebotRobstrideEnvClient(create_rpc_client(self.output_dir)) + self._client = client + config = load_config(getattr(self.args, "env_config", None)) + interval_s = min(0.5, config.heartbeat_timeout_s / 3.0) + self._heartbeat_stop.clear() + self._heartbeat_thread = threading.Thread( + target=self._heartbeat_loop, + args=(interval_s,), + daemon=True, + ) + self._heartbeat_thread.start() return RebotRobstrideToolkit(env=client, dashboard=self.dashboard) + def _heartbeat_loop(self, interval_s: float) -> None: + while not self._heartbeat_stop.is_set(): + client = self._client + if client is None: + return + try: + client.heartbeat() + except Exception: + if self._heartbeat_stop.is_set(): + return + self._heartbeat_stop.wait(interval_s) + def stop(self) -> None: + self._heartbeat_stop.set() + if self._heartbeat_thread is not None: + self._heartbeat_thread.join(timeout=2.0) + self._heartbeat_thread = None + self._client = None stop_socket_server_process( self._process, output_dir=self.output_dir, diff --git a/robots/rebot_robstride/toolkit.py b/robots/rebot_robstride/toolkit.py index 3faefac7..28438f59 100644 --- a/robots/rebot_robstride/toolkit.py +++ b/robots/rebot_robstride/toolkit.py @@ -11,15 +11,17 @@ "name": "get_robot_state", "description": ( "Read fresh RobStride joint, velocity, gripper, enable, and stop state. " - "Call this before enabling or planning any motion." + "Includes raw fault/warning reports. Call this before enabling or planning " + "any motion." ), "input_schema": {"type": "object", "properties": {}, "required": []}, }, { "name": "enable_arm", "description": ( - "Explicitly enable the arm after reading state. The driver clears faults, " - "selects MIT mode, and holds the observed pose before returning." + "Explicitly enable the six arm joints after reading state. The driver " + "validates startup feedback, clears and rechecks faults, selects MIT mode, " + "and holds the observed pose before returning." ), "input_schema": {"type": "object", "properties": {}, "required": []}, }, @@ -41,6 +43,7 @@ "duration_s": { "type": "number", "minimum": 0.1, + "maximum": 60.0, "default": 2.0, }, }, @@ -57,7 +60,12 @@ "type": "object", "properties": { "position": {"type": "number", "minimum": 0.0, "maximum": 1.0}, - "duration_s": {"type": "number", "minimum": 0.1, "default": 1.0}, + "duration_s": { + "type": "number", + "minimum": 0.1, + "maximum": 60.0, + "default": 1.0, + }, }, "required": ["position"], }, @@ -68,7 +76,12 @@ "input_schema": { "type": "object", "properties": { - "duration_s": {"type": "number", "minimum": 0.1, "default": 1.0} + "duration_s": { + "type": "number", + "minimum": 0.1, + "maximum": 60.0, + "default": 1.0, + } }, "required": [], }, @@ -79,7 +92,12 @@ "input_schema": { "type": "object", "properties": { - "duration_s": {"type": "number", "minimum": 0.1, "default": 1.0} + "duration_s": { + "type": "number", + "minimum": 0.1, + "maximum": 60.0, + "default": 1.0, + } }, "required": [], }, diff --git a/rpent/cli/main.py b/rpent/cli/main.py index 9c6a9cc2..61e2cee3 100644 --- a/rpent/cli/main.py +++ b/rpent/cli/main.py @@ -37,7 +37,7 @@ from pathlib import Path from rpent.cerebrum.base import build_cerebrum # noqa: E402 -from rpent.envs import get_env_spec, get_runtime # noqa: E402 +from rpent.envs import get_env_spec, get_runtime, validate_env_args # noqa: E402 from rpent.utils.config import ( get_repo_root, ) @@ -256,6 +256,7 @@ def main() -> int: logger.info("launcher config applied: %s", launch_config) env_name = args.env_name + validate_env_args(env_name, args, parser) suite = args.suite or env_name task = args.task if args.task is not None else 0 seed = args.seed @@ -344,6 +345,7 @@ def main() -> int: stats = result.stats agent_error = result.error except Exception as e: + agent_error = str(e) logger.error("EXCEPTION in agent loop: %s", e) finally: if toolkit is not None: @@ -362,6 +364,7 @@ def main() -> int: "model": args.model, "elapsed_s": round(elapsed, 1), "finish": finish_result, + "error": agent_error, "stats": stats, "messages": _serialize_messages(messages), } @@ -389,7 +392,7 @@ def main() -> int: threading.Event().wait() except KeyboardInterrupt: pass - return 0 + return 1 if agent_error else 0 if __name__ == "__main__": diff --git a/rpent/envs/__init__.py b/rpent/envs/__init__.py index d8726f37..a1eefff9 100644 --- a/rpent/envs/__init__.py +++ b/rpent/envs/__init__.py @@ -1,6 +1,6 @@ """Environment-specific RPent extensions.""" -from rpent.envs.base import get_env_spec, get_runtime, get_toolkit +from rpent.envs.base import get_env_spec, get_runtime, get_toolkit, validate_env_args from rpent.envs.env_spec import EnvSpec from rpent.envs.prompt_bundle import PromptBundle from rpent.envs.runtime import EnvRuntime @@ -12,4 +12,5 @@ "get_env_spec", "get_runtime", "get_toolkit", + "validate_env_args", ] diff --git a/rpent/envs/base.py b/rpent/envs/base.py index f381cdca..69f32660 100644 --- a/rpent/envs/base.py +++ b/rpent/envs/base.py @@ -17,9 +17,9 @@ from rpent.tools.toolkit import Toolkit from rpent.utils.config import get_repo_root -# Env packages live under ``/robots/``, which is not part of the installed -# ``rpent`` distribution. Ensure the repo root is importable so ``robots.`` -# resolves regardless of the process's current working directory. +# Source checkouts keep env packages under ``/robots/``. Installed wheels +# package the same namespace, while this path setup preserves checkout execution +# regardless of the process's current working directory. _REPO_ROOT = str(get_repo_root()) if _REPO_ROOT not in sys.path: sys.path.insert(0, _REPO_ROOT) @@ -53,3 +53,10 @@ def get_runtime(name: str, **kwargs): if factory is None: raise ValueError(f"environment {name!r} does not expose get_runtime") return factory(**kwargs) + + +def validate_env_args(name: str, args: Any, parser: Any) -> None: + """Run optional environment-specific CLI validation before side effects.""" + validator = getattr(_resolve_env(name), "validate_args", None) + if validator is not None: + validator(args, parser) diff --git a/rpent/utils/socket_rpc.py b/rpent/utils/socket_rpc.py index bdb9d5df..15c2c33b 100644 --- a/rpent/utils/socket_rpc.py +++ b/rpent/utils/socket_rpc.py @@ -8,6 +8,7 @@ Both processes are spawned by the same user on the same host, so we use pickle rather than a more defensive codec. """ + from __future__ import annotations import pickle @@ -19,7 +20,6 @@ from collections.abc import Callable from typing import Any - DEFAULT_CONNECT_TIMEOUT_S = 10.0 DEFAULT_REQUEST_TIMEOUT_S = 30.0 @@ -53,6 +53,7 @@ class RpcError(RuntimeError): """Raised when a remote method call returns an error.""" def __init__(self, method: str, message: str, *, traceback: str | None = None): + """Create an RPC error with optional remote traceback text.""" super().__init__(f"{method}: {message}") self.method = method self.server_traceback = traceback @@ -68,6 +69,7 @@ def __init__( *, connect_timeout_s: float = DEFAULT_CONNECT_TIMEOUT_S, ): + """Configure a client for one socket endpoint.""" self.host = host self.port = int(port) self.connect_timeout_s = connect_timeout_s @@ -80,6 +82,7 @@ def call( *, timeout_s: float | None = None, ) -> Any: + """Call one remote method and return its decoded result.""" req_id = str(uuid.uuid4()) payload = { "id": req_id, @@ -110,6 +113,7 @@ def call( return response.get("result") def close(self) -> None: + """Retain compatibility with clients that own persistent resources.""" return None @@ -129,6 +133,7 @@ def handle(self) -> None: response: dict = {"id": req_id, "ok": True, "result": result} except Exception as exc: import traceback as _tb + response = { "id": req_id, "ok": False, @@ -151,11 +156,18 @@ def __init__( self, server_address: tuple[str, int], dispatch: Callable[[str, tuple, dict], Any], + *, + priority_methods: set[str] | None = None, ): + """Create a server with optional lock-bypassing priority methods.""" super().__init__(server_address, _RequestHandler) self._dispatch = dispatch self._dispatch_lock = threading.Lock() + self._priority_methods = frozenset(priority_methods or ()) def dispatch(self, method: str, args: tuple, kwargs: dict) -> Any: + """Dispatch priority methods concurrently and serialize all others.""" + if method in self._priority_methods: + return self._dispatch(method, args, kwargs) with self._dispatch_lock: return self._dispatch(method, args, kwargs) diff --git a/scripts/check_rebot_robstride.py b/scripts/check_rebot_robstride.py index b02f956d..74ce3e1f 100644 --- a/scripts/check_rebot_robstride.py +++ b/scripts/check_rebot_robstride.py @@ -1,8 +1,8 @@ #!/usr/bin/env python3 """Passive seven-motor connectivity check for a reBot DevArm RobStride arm. -This script only reads RobStride mechPos/mechVel parameters. It never clears -faults, changes modes, enables motors, or sends a target. +This script only reads RobStride mechPos and fault-report telemetry. It never +clears faults, changes modes, enables motors, or sends a target. """ from __future__ import annotations diff --git a/tests/envs/test_runtime_registry.py b/tests/envs/test_runtime_registry.py index 82670231..801773c3 100644 --- a/tests/envs/test_runtime_registry.py +++ b/tests/envs/test_runtime_registry.py @@ -2,10 +2,11 @@ import sys import types +from argparse import ArgumentParser, Namespace import pytest -from rpent.envs import get_runtime +from rpent.envs import get_runtime, validate_env_args from rpent.envs.runtime import EnvRuntime @@ -42,3 +43,20 @@ def test_get_runtime_reports_missing_factory(monkeypatch: pytest.MonkeyPatch) -> with pytest.raises(ValueError, match="does not expose get_runtime"): get_runtime("no_runtime") + + +def test_libero_cli_validation_requires_suite_and_task() -> None: + parser = ArgumentParser() + + with pytest.raises(SystemExit) as exc_info: + validate_env_args("libero", Namespace(suite=None, task=None), parser) + + assert exc_info.value.code == 2 + + +def test_rebot_cli_validation_does_not_require_libero_arguments() -> None: + validate_env_args( + "rebot_robstride", + Namespace(suite=None, task=None), + ArgumentParser(), + ) diff --git a/tests/robots/rebot_robstride/test_config.py b/tests/robots/rebot_robstride/test_config.py index 0277fd18..e609ff13 100644 --- a/tests/robots/rebot_robstride/test_config.py +++ b/tests/robots/rebot_robstride/test_config.py @@ -58,3 +58,38 @@ def test_load_config_rejects_half_calibrated_gripper(tmp_path: Path) -> None: with pytest.raises(ValueError, match="both be set or both be null"): load_config(path) + + +@pytest.mark.parametrize( + ("field", "value"), + [ + ("control_rate_hz", ".inf"), + ("feedback_rate_hz", ".nan"), + ("settle_timeout_s", ".nan"), + ("max_motion_duration_s", ".inf"), + ], +) +def test_load_config_rejects_non_finite_global_values( + tmp_path: Path, field: str, value: str +) -> None: + path = tmp_path / "non-finite.yaml" + path.write_text(f"{field}: {value}\n") + + with pytest.raises(ValueError, match="finite and positive"): + load_config(path) + + +def test_load_config_rejects_unknown_root_key(tmp_path: Path) -> None: + path = tmp_path / "unknown.yaml" + path.write_text("control_rates_hz: 50\n") + + with pytest.raises(ValueError, match="unknown reBot config fields"): + load_config(path) + + +def test_load_config_rejects_non_numeric_nested_value(tmp_path: Path) -> None: + path = tmp_path / "bad-gripper.yaml" + path.write_text("gripper: {kp: fast}\n") + + with pytest.raises(ValueError, match="gripper contains a non-finite value"): + load_config(path) diff --git a/tests/robots/rebot_robstride/test_driver.py b/tests/robots/rebot_robstride/test_driver.py index 22fc71ef..20384535 100644 --- a/tests/robots/rebot_robstride/test_driver.py +++ b/tests/robots/rebot_robstride/test_driver.py @@ -1,96 +1,162 @@ from __future__ import annotations +import threading +import time from dataclasses import replace import pytest from robots.rebot_robstride.config import default_config -from robots.rebot_robstride.driver import RebotRobstrideDriver +from robots.rebot_robstride.driver import MotionCancelled, RebotRobstrideDriver MECH_POS = 0x7019 -MECH_VEL = 0x701A + + +class FakeClock: + def __init__(self) -> None: + self.now = 0.0 + self.lock = threading.Lock() + + def __call__(self) -> float: + with self.lock: + return self.now + + def sleep(self, duration: float) -> None: + with self.lock: + self.now += max(0.0, duration) class FakeMotor: - def __init__(self, motor_id: int, position: float = 0.0) -> None: + def __init__( + self, + motor_id: int, + *, + position: float = 0.0, + clock=time.monotonic, + ) -> None: self.motor_id = motor_id self.position = position - self.velocity = 0.0 + self.clock = clock self.calls: list[tuple] = [] + self.enabled = False self.fail_on_send = False + self.follow_commands = True + self.fault_raw = 0 + self.warning_raw = 0 + self.fault_on_send = 0 + self.clear_fault_on_clear = True + self.send_event = threading.Event() + self.block_enable = False + self.enable_entered = threading.Event() + self.release_enable = threading.Event() def robstride_get_param_f32(self, parameter: int, timeout_ms: int = 1000) -> float: self.calls.append(("read", parameter, timeout_ms)) - if parameter == MECH_POS: - return self.position - if parameter == MECH_VEL: - return self.velocity - raise AssertionError(f"unexpected parameter {parameter:#x}") + if parameter != MECH_POS: + raise AssertionError(f"unexpected parameter {parameter:#x}") + return self.position + + def robstride_get_fault_report(self) -> tuple[int, int]: + self.calls.append(("fault_report",)) + return self.fault_raw, self.warning_raw def clear_error(self) -> None: self.calls.append(("clear_error",)) + if self.clear_fault_on_clear: + self.fault_raw = 0 + self.warning_raw = 0 def ensure_mode(self, mode, timeout_ms: int = 1000) -> None: self.calls.append(("ensure_mode", int(mode), timeout_ms)) + def enable(self) -> None: + self.calls.append(("enable",)) + if self.block_enable: + self.enable_entered.set() + if not self.release_enable.wait(timeout=2.0): + raise TimeoutError("test did not release motor enable") + self.enabled = True + def send_mit( self, pos: float, vel: float, kp: float, kd: float, tau: float ) -> None: if self.fail_on_send: raise RuntimeError("injected send failure") - self.calls.append(("send_mit", pos, vel, kp, kd, tau)) - self.velocity = pos - self.position - self.position = pos + self.calls.append(("send_mit", pos, vel, kp, kd, tau, self.clock())) + self.send_event.set() + if self.fault_on_send: + self.fault_raw = self.fault_on_send + if self.follow_commands: + self.position = pos class FakeController: - def __init__(self, channel: str, positions: dict[int, float] | None = None) -> None: + def __init__( + self, + channel: str, + positions: dict[int, float] | None = None, + *, + clock=time.monotonic, + ) -> None: self.channel = channel self.positions = positions or {} + self.clock = clock self.motors: dict[int, FakeMotor] = {} - self.enabled = False self.disabled = False self.closed = False + @property + def enabled(self) -> bool: + return any(motor.enabled for motor in self.motors.values()) + def add_robstride_motor( self, motor_id: int, feedback_id: int, model: str ) -> FakeMotor: assert feedback_id == 0xFD - motor = FakeMotor(motor_id, self.positions.get(motor_id, 0.0)) + motor = FakeMotor( + motor_id, + position=self.positions.get(motor_id, 0.0), + clock=self.clock, + ) motor.calls.append(("registered", feedback_id, model)) self.motors[motor_id] = motor return motor - def enable_all(self) -> None: - self.enabled = True - def disable_all(self) -> None: self.disabled = True - self.enabled = False + for motor in self.motors.values(): + motor.enabled = False def close(self) -> None: self.closed = True -def make_driver(positions: dict[int, float] | None = None): +def make_driver( + positions: dict[int, float] | None = None, + *, + config=None, + clock: FakeClock | None = None, +): + fake_clock = clock or FakeClock() controllers: list[FakeController] = [] def factory(channel: str) -> FakeController: - controller = FakeController(channel, positions) + controller = FakeController(channel, positions, clock=fake_clock) controllers.append(controller) return controller driver = RebotRobstrideDriver( - default_config(), + config or default_config(), controller_factory=factory, - sleep=lambda _: None, + clock=fake_clock, + sleep=fake_clock.sleep, ) - return driver, controllers + return driver, controllers, fake_clock def test_connect_is_passive_and_reads_every_motor() -> None: expected = {motor_id: motor_id / 100 for motor_id in range(1, 8)} - driver, controllers = make_driver(expected) + driver, controllers, _ = make_driver(expected) state = driver.connect() @@ -102,44 +168,105 @@ def test_connect_is_passive_and_reads_every_motor() -> None: assert state["gripper_position"] == pytest.approx(expected[7]) for motor in controller.motors.values(): assert any(call[:2] == ("read", MECH_POS) for call in motor.calls) + assert ("fault_report",) in motor.calls + assert ("enable",) not in motor.calls driver.close() assert controller.disabled is False assert controller.closed is True -def test_enable_holds_observed_pose_before_accepting_motion() -> None: - positions = {motor_id: -0.01 * motor_id for motor_id in range(1, 8)} - driver, controllers = make_driver(positions) +def test_enable_validates_then_holds_only_arm_motors() -> None: + positions = {motor_id: -0.01 * motor_id for motor_id in range(1, 7)} | {7: 0.0} + driver, controllers, _ = make_driver(positions) driver.connect() result = driver.enable() controller = controllers[0] assert result["enabled"] is True - assert controller.enabled - for motor_id, motor in controller.motors.items(): - names = [call[0] for call in motor.calls] - assert "clear_error" in names - assert "ensure_mode" in names + assert result["gripper_enabled"] is False + for motor_id in range(1, 7): + motor = controller.motors[motor_id] + assert ("clear_error",) in motor.calls + assert ("enable",) in motor.calls hold = next(call for call in motor.calls if call[0] == "send_mit") assert hold[1] == pytest.approx(positions[motor_id]) + assert ("enable",) not in controller.motors[7].calls + assert not controller.motors[7].enabled + + +def test_enable_rejects_out_of_limit_startup_before_torque() -> None: + driver, controllers, _ = make_driver({2: 0.2}) + driver.connect() + + with pytest.raises(ValueError, match="joint2"): + driver.enable() + + assert not controllers[0].enabled + assert all( + ("enable",) not in motor.calls for motor in controllers[0].motors.values() + ) + + +def test_enable_rejects_persistent_fault_before_torque() -> None: + driver, controllers, _ = make_driver() + driver.connect() + motor = controllers[0].motors[3] + motor.fault_raw = 4 + motor.clear_fault_on_clear = False + + with pytest.raises(RuntimeError, match="fault"): + driver.enable() + + assert controllers[0].disabled + assert not controllers[0].enabled def test_enable_failure_disables_all_motors() -> None: - driver, controllers = make_driver() + driver, controllers, _ = make_driver() driver.connect() controllers[0].motors[1].fail_on_send = True with pytest.raises(RuntimeError, match="injected send failure"): driver.enable() - assert controllers[0].disabled is True + assert controllers[0].disabled assert driver.state()["enabled"] is False -def test_move_joints_interpolates_and_returns_readback_evidence() -> None: - driver, controllers = make_driver() +def test_emergency_stop_cancels_an_enable_in_progress() -> None: + driver, controllers, _ = make_driver() + driver.connect() + blocked_motor = controllers[0].motors[1] + blocked_motor.block_enable = True + enable_error: list[BaseException] = [] + + def enable() -> None: + try: + driver.enable() + except BaseException as exc: # captured for the test thread + enable_error.append(exc) + + enable_thread = threading.Thread(target=enable) + enable_thread.start() + assert blocked_motor.enable_entered.wait(timeout=1.0) + + stop_thread = threading.Thread(target=driver.emergency_stop) + stop_thread.start() + blocked_motor.release_enable.set() + enable_thread.join(timeout=1.0) + stop_thread.join(timeout=1.0) + + assert not enable_thread.is_alive() + assert not stop_thread.is_alive() + assert enable_error and isinstance(enable_error[0], MotionCancelled) + assert controllers[0].disabled + assert not controllers[0].enabled + + +def test_move_joints_interpolates_and_returns_settled_evidence() -> None: + driver, controllers, _ = make_driver() driver.connect() driver.enable() target = [0.1, -0.1, -0.1, 0.05, -0.05, 0.1] @@ -148,6 +275,7 @@ def test_move_joints_interpolates_and_returns_readback_evidence() -> None: assert result["reached"] is True assert result["final_positions"] == pytest.approx(target) + assert result["final_velocities"] == pytest.approx([0.0] * 6) assert result["max_error"] == pytest.approx(0.0) for motor_id in range(1, 7): commands = [ @@ -155,11 +283,30 @@ def test_move_joints_interpolates_and_returns_readback_evidence() -> None: for call in controllers[0].motors[motor_id].calls if call[0] == "send_mit" ] - assert len(commands) >= 3 # initial hold plus at least two trajectory points + assert len(commands) >= 3 -def test_motion_transport_failure_latches_soft_stop() -> None: - driver, controllers = make_driver() +def test_minimum_jerk_profile_respects_configured_velocity_cap() -> None: + driver, controllers, _ = make_driver() + driver.connect() + driver.enable() + + result = driver.move_joints([0.5, 0.0, 0.0, 0.0, 0.0, 0.0], duration_s=0.1) + + commands = [ + call for call in controllers[0].motors[1].calls if call[0] == "send_mit" + ] + velocities = [ + abs(current[1] - previous[1]) / (current[6] - previous[6]) + for previous, current in zip(commands, commands[1:]) + if current[6] > previous[6] + ] + assert max(velocities) <= driver.config.joints[0].max_velocity * 1.02 + assert result["actual_duration_s"] >= 1.875 + + +def test_motion_transport_failure_disables_all_and_latches_stop() -> None: + driver, controllers, _ = make_driver() driver.connect() driver.enable() controllers[0].motors[3].fail_on_send = True @@ -168,23 +315,68 @@ def test_motion_transport_failure_latches_soft_stop() -> None: driver.move_joints([0.1, -0.1, -0.1, 0.0, 0.0, 0.0], duration_s=1.0) state = driver.state() - assert state["enabled"] is True + assert state["enabled"] is False assert state["stopped"] is True - with pytest.raises(RuntimeError, match="stopped"): - driver.move_joints([0.0] * 6, duration_s=1.0) + assert controllers[0].disabled + + +def test_motion_fault_report_disables_all() -> None: + driver, controllers, _ = make_driver() + driver.connect() + driver.enable() + controllers[0].motors[3].fault_on_send = 8 + + with pytest.raises(RuntimeError, match="fault during motion"): + driver.move_joints([0.1, -0.1, -0.1, 0.0, 0.0, 0.0], duration_s=1.0) + + assert controllers[0].disabled + assert driver.state()["enabled"] is False + + +def test_stalled_feedback_aborts_and_disables() -> None: + driver, controllers, _ = make_driver() + driver.connect() + driver.enable() + controllers[0].motors[1].follow_commands = False + + with pytest.raises(RuntimeError, match="tracking error"): + driver.move_joints([0.6, 0.0, 0.0, 0.0, 0.0, 0.0], duration_s=0.1) + + assert controllers[0].disabled + assert driver.state()["enabled"] is False -def test_move_joints_rejects_limit_violation() -> None: - driver, _ = make_driver() +def test_settlement_timeout_returns_unreached_and_disables() -> None: + config = replace( + default_config(), + max_tracking_error_rad=0.5, + settle_timeout_s=0.2, + ) + driver, controllers, _ = make_driver(config=config) + driver.connect() + driver.enable() + controllers[0].motors[1].follow_commands = False + + result = driver.move_joints([0.1, 0.0, 0.0, 0.0, 0.0, 0.0], duration_s=0.2) + + assert result["reached"] is False + assert result["enabled"] is False + assert controllers[0].disabled + + +def test_move_joints_rejects_limit_and_duration_violations() -> None: + driver, _, _ = make_driver() driver.connect() driver.enable() with pytest.raises(ValueError, match="joint2"): driver.move_joints([0.0, 0.1, -0.1, 0.0, 0.0, 0.0], duration_s=1.0) + with pytest.raises(ValueError, match="must not exceed"): + driver.move_joints([0.0] * 6, duration_s=61.0) def test_motion_requires_enable_and_respects_soft_stop() -> None: - driver, _ = make_driver() + driver, _, _ = make_driver() driver.connect() with pytest.raises(RuntimeError, match="not enabled"): @@ -198,79 +390,113 @@ def test_motion_requires_enable_and_respects_soft_stop() -> None: assert driver.move_joints([0.0] * 6, duration_s=0.1)["reached"] is True -def test_stop_failure_keeps_soft_stop_latched() -> None: - driver, controllers = make_driver() +def test_emergency_stop_preempts_active_trajectory() -> None: + controllers: list[FakeController] = [] + + def factory(channel: str) -> FakeController: + controller = FakeController(channel) + controllers.append(controller) + return controller + + driver = RebotRobstrideDriver(default_config(), controller_factory=factory) driver.connect() driver.enable() - controllers[0].motors[3].fail_on_send = True + for motor in controllers[0].motors.values(): + motor.send_event.clear() - with pytest.raises(RuntimeError, match="injected send failure"): - driver.stop_motion() + motion_error: list[BaseException] = [] - assert driver.state()["stopped"] is True + def move() -> None: + try: + driver.move_joints([0.5, 0.0, 0.0, 0.0, 0.0, 0.0], duration_s=2.0) + except BaseException as exc: # captured for the test thread + motion_error.append(exc) + thread = threading.Thread(target=move) + thread.start() + assert controllers[0].motors[1].send_event.wait(timeout=1.0) -def test_emergency_stop_disables_all_motors() -> None: - driver, controllers = make_driver() + started = time.monotonic() + result = driver.emergency_stop() + elapsed = time.monotonic() - started + thread.join(timeout=1.0) + + assert elapsed < 0.25 + assert result == {"enabled": False, "stopped": True} + assert controllers[0].disabled + assert not thread.is_alive() + assert motion_error and isinstance(motion_error[0], MotionCancelled) + + +def test_expired_agent_heartbeat_disables_enabled_motors() -> None: + driver, controllers, clock = make_driver() driver.connect() driver.enable() - result = driver.emergency_stop() + assert driver.enforce_heartbeat_deadman() is False + clock.sleep(driver.config.heartbeat_timeout_s + 0.01) - assert result == {"enabled": False, "stopped": True} + assert driver.enforce_heartbeat_deadman() is True assert controllers[0].disabled + assert driver.state()["enabled"] is False def test_gripper_refuses_motion_until_endpoints_are_calibrated() -> None: - driver, _ = make_driver() + driver, controllers, _ = make_driver() driver.connect() driver.enable() with pytest.raises(RuntimeError, match="not calibrated"): driver.set_gripper(0.5) + assert not controllers[0].motors[7].enabled -def test_gripper_maps_normalized_position_to_calibrated_range() -> None: + +def test_gripper_maps_and_returns_settlement_evidence() -> None: config = default_config() calibrated = replace( config, gripper=replace(config.gripper, open_position=-1.0, closed_position=0.0), ) - controller = FakeController("can0") - driver = RebotRobstrideDriver( - calibrated, - controller_factory=lambda _: controller, - sleep=lambda _: None, - ) + driver, controllers, _ = make_driver(config=calibrated) driver.connect() driver.enable() - with pytest.raises(ValueError, match="duration_s"): - driver.set_gripper(0.25, duration_s=0.0) - result = driver.set_gripper(0.25) assert result["target_position"] == pytest.approx(-0.75) - assert controller.motors[7].position == pytest.approx(-0.75) + assert result["final_position"] == pytest.approx(-0.75) + assert result["final_velocity"] == pytest.approx(0.0) + assert result["max_error"] == pytest.approx(0.0) + assert result["reached"] is True + assert controllers[0].motors[7].enabled -def test_gripper_transport_failure_latches_soft_stop() -> None: +def test_gripper_transport_failure_disables_all() -> None: config = default_config() calibrated = replace( config, gripper=replace(config.gripper, open_position=-1.0, closed_position=0.0), ) - controller = FakeController("can0") - driver = RebotRobstrideDriver( - calibrated, - controller_factory=lambda _: controller, - sleep=lambda _: None, - ) + driver, controllers, _ = make_driver(config=calibrated) driver.connect() driver.enable() - controller.motors[7].fail_on_send = True + controllers[0].motors[7].fail_on_send = True with pytest.raises(RuntimeError, match="injected send failure"): driver.set_gripper(0.5) assert driver.state()["stopped"] is True + assert driver.state()["enabled"] is False + assert controllers[0].disabled + + +def test_close_always_disables_if_any_motor_was_enabled() -> None: + driver, controllers, _ = make_driver() + driver.connect() + driver.enable() + + driver.close() + + assert controllers[0].disabled + assert controllers[0].closed diff --git a/tests/robots/rebot_robstride/test_env_client.py b/tests/robots/rebot_robstride/test_env_client.py index 81ec7ece..9655540a 100644 --- a/tests/robots/rebot_robstride/test_env_client.py +++ b/tests/robots/rebot_robstride/test_env_client.py @@ -28,3 +28,4 @@ def test_client_uses_stable_robot_rpc_names() -> None: assert client.stop_motion()["method"] == "robot.stop_motion" assert client.reset_stop()["method"] == "robot.reset_stop" assert client.emergency_stop()["method"] == "robot.emergency_stop" + assert client.heartbeat()["method"] == "robot.heartbeat" diff --git a/tests/robots/rebot_robstride/test_env_server.py b/tests/robots/rebot_robstride/test_env_server.py index 9fbe86cd..fb54ad46 100644 --- a/tests/robots/rebot_robstride/test_env_server.py +++ b/tests/robots/rebot_robstride/test_env_server.py @@ -4,7 +4,7 @@ import pytest -from robots.rebot_robstride.env_server import make_dispatch +from robots.rebot_robstride.env_server import make_dispatch, validate_loopback_host class FakeDriver: @@ -29,6 +29,9 @@ def reset_stop(self): def emergency_stop(self): return {"op": "estop"} + def heartbeat(self): + return {"op": "heartbeat"} + def test_dispatch_maps_only_declared_robot_methods() -> None: shutdown = threading.Event() @@ -51,14 +54,26 @@ def test_dispatch_maps_only_declared_robot_methods() -> None: assert dispatch("robot.stop_motion", (), {}) == {"op": "stop"} assert dispatch("robot.reset_stop", (), {}) == {"op": "reset"} assert dispatch("robot.emergency_stop", (), {}) == {"op": "estop"} + assert dispatch("robot.heartbeat", (), {}) == {"op": "heartbeat"} with pytest.raises(ValueError, match="unknown RPC method"): dispatch("robot.set_zero", (), {}) -def test_shutdown_sets_event_without_touching_motion() -> None: +def test_shutdown_sets_event_after_emergency_stop() -> None: shutdown = threading.Event() dispatch = make_dispatch(FakeDriver(), shutdown) assert dispatch("shutdown", (), {}) == {"ok": True} assert shutdown.is_set() + + +@pytest.mark.parametrize("host", ["127.0.0.1", "127.0.0.2", "::1", "localhost"]) +def test_loopback_hosts_are_accepted(host: str) -> None: + validate_loopback_host(host) + + +@pytest.mark.parametrize("host", ["0.0.0.0", "192.168.1.4", "rpent.example"]) +def test_non_loopback_hosts_are_rejected(host: str) -> None: + with pytest.raises(ValueError, match="loopback"): + validate_loopback_host(host) diff --git a/tests/robots/test_runtimes.py b/tests/robots/test_runtimes.py index a6be468c..4443f47d 100644 --- a/tests/robots/test_runtimes.py +++ b/tests/robots/test_runtimes.py @@ -1,5 +1,6 @@ from __future__ import annotations +import threading from argparse import Namespace from typing import cast @@ -18,6 +19,16 @@ def __init__(self, rpc, *, expected_meta) -> None: self.expected_meta = expected_meta +class FakeRebotEnv: + def __init__(self, rpc) -> None: + self.rpc = rpc + self.heartbeat_seen = threading.Event() + + def heartbeat(self) -> dict: + self.heartbeat_seen.set() + return {"ok": True} + + def test_libero_runtime_preserves_no_driver_attach(monkeypatch, tmp_path) -> None: endpoint_calls: list[tuple] = [] fake_rpc = object() @@ -70,9 +81,7 @@ def test_rebot_runtime_supports_no_driver_attach(monkeypatch, tmp_path) -> None: lambda output, host, port: endpoint_calls.append((output, host, port)), ) monkeypatch.setattr(rebot_runtime_module, "create_rpc_client", lambda _: fake_rpc) - monkeypatch.setattr( - rebot_runtime_module, "RebotRobstrideEnvClient", lambda rpc: ("env", rpc) - ) + monkeypatch.setattr(rebot_runtime_module, "RebotRobstrideEnvClient", FakeRebotEnv) monkeypatch.setattr(rebot_runtime_module, "RebotRobstrideToolkit", FakeToolkit) args = Namespace( no_driver=True, @@ -87,4 +96,51 @@ def test_rebot_runtime_supports_no_driver_attach(monkeypatch, tmp_path) -> None: toolkit = cast(FakeToolkit, runtime.start()) assert endpoint_calls == [(tmp_path, "127.0.0.1", 46001)] - assert toolkit.kwargs["env"] == ("env", fake_rpc) + env = toolkit.kwargs["env"] + assert isinstance(env, FakeRebotEnv) + assert env.rpc is fake_rpc + assert env.heartbeat_seen.wait(timeout=1.0) + runtime.stop() + + +def test_rebot_runtime_stops_the_spawned_server(monkeypatch, tmp_path) -> None: + fake_process = object() + starts: list[tuple] = [] + stops: list[tuple] = [] + fake_rpc = object() + + def start_process(command, **kwargs): + starts.append((command, kwargs)) + return fake_process + + def stop_process(process, **kwargs) -> None: + stops.append((process, kwargs)) + + monkeypatch.setattr( + rebot_runtime_module, "start_socket_server_process", start_process + ) + monkeypatch.setattr( + rebot_runtime_module, "stop_socket_server_process", stop_process + ) + monkeypatch.setattr(rebot_runtime_module, "create_rpc_client", lambda _: fake_rpc) + monkeypatch.setattr(rebot_runtime_module, "RebotRobstrideEnvClient", FakeRebotEnv) + monkeypatch.setattr(rebot_runtime_module, "RebotRobstrideToolkit", FakeToolkit) + args = Namespace( + no_driver=False, + env_endpoint="127.0.0.1", + env_port=0, + env_config=None, + ) + runtime = rebot_runtime_module.RebotRobstrideRuntime( + args=args, output_dir=tmp_path, dashboard=None + ) + + toolkit = cast(FakeToolkit, runtime.start()) + env = toolkit.kwargs["env"] + assert isinstance(env, FakeRebotEnv) + assert env.heartbeat_seen.wait(timeout=1.0) + assert starts and starts[0][0][-1].endswith("robots/rebot_robstride/env_server.py") + + runtime.stop() + + assert stops == [(fake_process, {"output_dir": tmp_path})] diff --git a/tests/utils/test_socket_rpc_priority.py b/tests/utils/test_socket_rpc_priority.py new file mode 100644 index 00000000..0b7eaf4f --- /dev/null +++ b/tests/utils/test_socket_rpc_priority.py @@ -0,0 +1,46 @@ +from __future__ import annotations + +import threading + +from rpent.utils.socket_rpc import SocketRpcClient, SocketRpcServer + + +def test_priority_rpc_bypasses_serialized_command_lock() -> None: + slow_started = threading.Event() + release_slow = threading.Event() + slow_result: list[object] = [] + + def dispatch(method: str, _args: tuple, _kwargs: dict): + if method == "slow": + slow_started.set() + if not release_slow.wait(timeout=2.0): + raise TimeoutError("test did not release slow RPC") + return "slow-finished" + if method == "stop": + return "stopped" + raise ValueError(method) + + server = SocketRpcServer(("127.0.0.1", 0), dispatch, priority_methods={"stop"}) + server_thread = threading.Thread(target=server.serve_forever) + server_thread.start() + port = int(server.server_address[1]) + client = SocketRpcClient("127.0.0.1", port) + + def call_slow() -> None: + slow_result.append(client.call("slow", timeout_s=2.0)) + + slow_thread = threading.Thread(target=call_slow) + slow_thread.start() + assert slow_started.wait(timeout=1.0) + + try: + assert client.call("stop", timeout_s=0.25) == "stopped" + assert slow_thread.is_alive() + finally: + release_slow.set() + slow_thread.join(timeout=1.0) + server.shutdown() + server.server_close() + server_thread.join(timeout=1.0) + + assert slow_result == ["slow-finished"] From 7282f8d0922907d0ad267f75514545eb5206f42d Mon Sep 17 00:00:00 2001 From: johnnynunez Date: Mon, 20 Jul 2026 04:45:23 +0200 Subject: [PATCH 3/6] fix(rebot): close final enable race Publish arm and gripper enabled state only after a final cancellation check under the I/O and state locks, so a concurrent emergency stop always wins. Reject overlong gripper profiles before lazy enable. --- robots/rebot_robstride/driver.py | 27 +++++----- tests/robots/rebot_robstride/test_driver.py | 60 +++++++++++++++++++++ 2 files changed, 75 insertions(+), 12 deletions(-) diff --git a/robots/rebot_robstride/driver.py b/robots/rebot_robstride/driver.py index 01937219..23a6cb92 100644 --- a/robots/rebot_robstride/driver.py +++ b/robots/rebot_robstride/driver.py @@ -198,15 +198,18 @@ def enable(self) -> dict[str, Any]: except Exception as exc: self._fail_closed(exc) - with self._state_lock: - self._enabled = True - self._gripper_enabled = False - self._stopped = False - self._cancel_event.clear() - self._last_heartbeat = self._clock() - self._last_targets = [ - second["positions"][joint.motor_id] for joint in arm_configs - ] + try: + with self._io_lock, self._state_lock: + self._raise_if_cancelled() + self._enabled = True + self._gripper_enabled = False + self._stopped = False + self._last_heartbeat = self._clock() + self._last_targets = [ + second["positions"][joint.motor_id] for joint in arm_configs + ] + except Exception as exc: + self._fail_closed(exc) return { "enabled": True, "gripper_enabled": False, @@ -336,13 +339,12 @@ def set_gripper( target = gripper.open_position + normalized * ( gripper.closed_position - gripper.open_position ) - self._ensure_gripper_enabled(gripper, start) - minimum_duration = ( MIN_JERK_PEAK_VELOCITY * abs(target - start) / gripper.max_velocity ) actual_duration = max(requested_duration, minimum_duration) self._check_actual_duration(actual_duration) + self._ensure_gripper_enabled(gripper, start) steps = max(2, math.ceil(actual_duration * self.config.control_rate_hz)) interval = actual_duration / steps feedback_stride = max( @@ -632,7 +634,8 @@ def _ensure_gripper_enabled(self, gripper: GripperConfig, hold: float) -> None: with self._io_lock: self._raise_if_cancelled() self._send_mit_locked(gripper, hold) - with self._state_lock: + with self._io_lock, self._state_lock: + self._raise_if_cancelled() self._gripper_enabled = True except Exception as exc: self._fail_closed(exc) diff --git a/tests/robots/rebot_robstride/test_driver.py b/tests/robots/rebot_robstride/test_driver.py index 20384535..48afb65f 100644 --- a/tests/robots/rebot_robstride/test_driver.py +++ b/tests/robots/rebot_robstride/test_driver.py @@ -49,6 +49,9 @@ def __init__( self.block_enable = False self.enable_entered = threading.Event() self.release_enable = threading.Event() + self.block_send = False + self.send_entered = threading.Event() + self.release_send = threading.Event() def robstride_get_param_f32(self, parameter: int, timeout_ms: int = 1000) -> float: self.calls.append(("read", parameter, timeout_ms)) @@ -84,6 +87,10 @@ def send_mit( raise RuntimeError("injected send failure") self.calls.append(("send_mit", pos, vel, kp, kd, tau, self.clock())) self.send_event.set() + if self.block_send: + self.send_entered.set() + if not self.release_send.wait(timeout=2.0): + raise TimeoutError("test did not release motor send") if self.fault_on_send: self.fault_raw = self.fault_on_send if self.follow_commands: @@ -265,6 +272,36 @@ def enable() -> None: assert not controllers[0].enabled +def test_emergency_stop_wins_after_the_last_enable_hold() -> None: + driver, controllers, _ = make_driver() + driver.connect() + last_arm_motor = controllers[0].motors[6] + last_arm_motor.block_send = True + enable_error: list[BaseException] = [] + + def enable() -> None: + try: + driver.enable() + except BaseException as exc: # captured for the test thread + enable_error.append(exc) + + enable_thread = threading.Thread(target=enable) + enable_thread.start() + assert last_arm_motor.send_entered.wait(timeout=1.0) + + stop_thread = threading.Thread(target=driver.emergency_stop) + stop_thread.start() + last_arm_motor.release_send.set() + enable_thread.join(timeout=1.0) + stop_thread.join(timeout=1.0) + + assert not enable_thread.is_alive() + assert not stop_thread.is_alive() + assert enable_error and isinstance(enable_error[0], MotionCancelled) + assert controllers[0].disabled + assert driver.state()["enabled"] is False + + def test_move_joints_interpolates_and_returns_settled_evidence() -> None: driver, controllers, _ = make_driver() driver.connect() @@ -452,6 +489,29 @@ def test_gripper_refuses_motion_until_endpoints_are_calibrated() -> None: assert not controllers[0].motors[7].enabled +def test_gripper_rejects_excessive_duration_before_enable() -> None: + config = default_config() + calibrated = replace( + config, + gripper=replace( + config.gripper, + open_position=0.0, + closed_position=-1.0, + max_velocity=0.01, + ), + ) + driver, controllers, _ = make_driver(config=calibrated) + driver.connect() + driver.enable() + + with pytest.raises(ValueError, match="exceeds max_motion_duration_s"): + driver.set_gripper(1.0) + + gripper = controllers[0].motors[7] + assert not gripper.enabled + assert not any(call[0] == "enable" for call in gripper.calls) + + def test_gripper_maps_and_returns_settlement_evidence() -> None: config = default_config() calibrated = replace( From 0c464457096022f802065b2bd34fde603db86149 Mon Sep 17 00:00:00 2001 From: johnnynunez Date: Mon, 20 Jul 2026 04:56:59 +0200 Subject: [PATCH 4/6] fix(rebot): close remaining safety races Prevent motion entry from clearing concurrent stop requests, hold an enabled gripper during soft stop, and latch disable failures until a confirmed emergency-stop retry. Use wall-clock trajectory progress to avoid feedback-latency catch-up bursts and enforce hard configuration ceilings within the 75-second RPC budget. --- .../rst_source/usage/rebot_robstride.rst | 11 ++ robots/rebot_robstride/config.py | 54 +++++-- robots/rebot_robstride/driver.py | 140 +++++++++++------- robots/rebot_robstride/toolkit.py | 6 +- tests/robots/rebot_robstride/test_config.py | 22 +++ tests/robots/rebot_robstride/test_driver.py | 124 ++++++++++++++++ 6 files changed, 292 insertions(+), 65 deletions(-) diff --git a/docs/source-en/rst_source/usage/rebot_robstride.rst b/docs/source-en/rst_source/usage/rebot_robstride.rst index 47d7de93..74c0e0a4 100644 --- a/docs/source-en/rst_source/usage/rebot_robstride.rst +++ b/docs/source-en/rst_source/usage/rebot_robstride.rst @@ -73,6 +73,13 @@ Gripper ``open_position`` and ``closed_position`` intentionally default to null. Calibrate the installed gripper before setting them. RPent refuses every gripper command while either endpoint is missing. +Safety-critical overrides have hard ceilings: control is 10–200 Hz, feedback +is at least 5 Hz, motion is at most 60 seconds, settlement is at most 5 seconds, +and their combined budget is at most 65 seconds versus the 75-second motion RPC +timeout. Heartbeat timeout is 0.25–5 seconds, joint/gripper velocity is at most +1 rad/s, and MIT gains are capped at ``kp=200`` / ``kd=20``. Configuration that +exceeds these bounds is rejected before opening SocketCAN. + Run --- @@ -126,6 +133,10 @@ use a priority RPC path that bypasses the serialized motion-command lock. Trajectories check a cancellation latch at every control tick. A runtime worker sends periodic heartbeats; if the agent process disappears while torque is enabled, the server calls emergency stop after ``heartbeat_timeout_s``. +Soft stop holds both the arm and an enabled gripper at freshly measured +positions. If ``disable_all`` itself fails, state reports ``disable_failed=true`` +and ``reset_stop`` remains blocked until a later emergency-stop retry confirms +that torque was removed. The pickle-framed hardware RPC server accepts loopback binds only. Do not expose it through a TCP proxy or bind it to a LAN address. diff --git a/robots/rebot_robstride/config.py b/robots/rebot_robstride/config.py index 5f993dc7..cc4419f3 100644 --- a/robots/rebot_robstride/config.py +++ b/robots/rebot_robstride/config.py @@ -205,18 +205,39 @@ def _validate(config: RebotConfig) -> RebotConfig: or value <= 0 ): raise ValueError(f"{name} must be finite and positive") - if config.control_rate_hz > 200: - raise ValueError("control_rate_hz must not exceed 200 Hz") - if config.feedback_rate_hz > config.control_rate_hz: - raise ValueError("feedback_rate_hz must not exceed control_rate_hz") + if not 10 <= config.control_rate_hz <= 200: + raise ValueError("control_rate_hz must be in [10, 200]") + if not 5 <= config.feedback_rate_hz <= config.control_rate_hz: + raise ValueError("feedback_rate_hz must be in [5, control_rate_hz]") if ( isinstance(config.settle_samples, bool) or not isinstance(config.settle_samples, int) - or config.settle_samples < 1 + or not 1 <= config.settle_samples <= 10 ): - raise ValueError("settle_samples must be a positive integer") - if config.max_motion_duration_s >= 120: - raise ValueError("max_motion_duration_s must remain below the RPC timeout") + raise ValueError("settle_samples must be an integer in [1, 10]") + bounded_values = { + "settle_tolerance": (config.settle_tolerance, 0.2), + "settle_timeout_s": (config.settle_timeout_s, 5.0), + "settle_velocity_rad_s": (config.settle_velocity_rad_s, 0.5), + "startup_sample_interval_s": (config.startup_sample_interval_s, 1.0), + "startup_velocity_limit_rad_s": ( + config.startup_velocity_limit_rad_s, + 0.5, + ), + "max_tracking_error_rad": (config.max_tracking_error_rad, 1.0), + "velocity_abort_multiplier": (config.velocity_abort_multiplier, 5.0), + "max_motion_duration_s": (config.max_motion_duration_s, 60.0), + "heartbeat_timeout_s": (config.heartbeat_timeout_s, 5.0), + } + for name, (value, maximum) in bounded_values.items(): + if value > maximum: + raise ValueError(f"{name} must not exceed {maximum}") + if config.heartbeat_timeout_s < 0.25: + raise ValueError("heartbeat_timeout_s must be at least 0.25") + if config.max_motion_duration_s + config.settle_timeout_s > 65.0: + raise ValueError( + "max_motion_duration_s + settle_timeout_s must not exceed 65 seconds" + ) if len(config.joints) != 6: raise ValueError("exactly six arm joints are required") @@ -246,8 +267,12 @@ def _validate(config: RebotConfig) -> RebotConfig: raise ValueError(f"{joint.name} contains a non-finite value") if joint.lower >= joint.upper: raise ValueError(f"{joint.name} lower limit must be below upper limit") - if joint.kp < 0 or joint.kd < 0: - raise ValueError(f"{joint.name} gains must be non-negative") + if max(abs(joint.lower), abs(joint.upper)) > 2 * math.pi: + raise ValueError(f"{joint.name} limits must remain within +/-2*pi") + if not 0 <= joint.kp <= 200 or not 0 <= joint.kd <= 20: + raise ValueError( + f"{joint.name} gains must satisfy kp in [0, 200], kd in [0, 20]" + ) if not 0 < joint.max_velocity <= 1.0: raise ValueError(f"{joint.name} max_velocity must be in (0, 1.0]") @@ -264,9 +289,12 @@ def _validate(config: RebotConfig) -> RebotConfig: for value in gripper_values ): raise ValueError("gripper contains a non-finite value") - if gripper.kp < 0 or gripper.kd < 0 or gripper.max_velocity <= 0: - raise ValueError("gripper gains must be non-negative and max_velocity positive") + if not 0 <= gripper.kp <= 200 or not 0 <= gripper.kd <= 20: + raise ValueError("gripper gains must satisfy kp in [0, 200], kd in [0, 20]") + if not 0 < gripper.max_velocity <= 1.0: + raise ValueError("gripper max_velocity must be in (0, 1.0]") if gripper.open_position is not None: + assert gripper.closed_position is not None endpoints = (gripper.open_position, gripper.closed_position) if not all( isinstance(value, (int, float)) @@ -277,4 +305,6 @@ def _validate(config: RebotConfig) -> RebotConfig: raise ValueError("gripper endpoints must be finite") if gripper.open_position == gripper.closed_position: raise ValueError("gripper endpoints must differ") + if max(abs(value) for value in endpoints) > 4 * math.pi: + raise ValueError("gripper endpoints must remain within +/-4*pi") return config diff --git a/robots/rebot_robstride/driver.py b/robots/rebot_robstride/driver.py index 23a6cb92..55d5d8d8 100644 --- a/robots/rebot_robstride/driver.py +++ b/robots/rebot_robstride/driver.py @@ -62,6 +62,7 @@ def __init__( self._enabled = False self._gripper_enabled = False self._stopped = False + self._disable_failed = False self._last_targets: list[float] | None = None self._previous_positions: dict[int, float] | None = None self._previous_sample_time: float | None = None @@ -107,6 +108,7 @@ def connect(self) -> dict[str, Any]: with self._state_lock: self._controller = controller self._motors = motors + self._disable_failed = False snapshot = self.state() self._last_targets = list(snapshot["joint_positions"]) return snapshot @@ -126,6 +128,7 @@ def state(self) -> dict[str, Any]: enabled = self._enabled gripper_enabled = self._gripper_enabled stopped = self._stopped + disable_failed = self._disable_failed joint_ids = [joint.motor_id for joint in self.config.joints] gripper_id = self.config.gripper.motor_id return { @@ -133,6 +136,7 @@ def state(self) -> dict[str, Any]: "enabled": enabled, "gripper_enabled": gripper_enabled, "stopped": stopped, + "disable_failed": disable_failed, "channel": self.config.channel, "joint_names": [joint.name for joint in self.config.joints], "joint_positions": [ @@ -233,7 +237,6 @@ def move_joints( self._validate_joint_limits(target) requested_duration = self._validate_duration(duration_s) - self._cancel_event.clear() try: start_state = self._sample_feedback() start = [ @@ -249,18 +252,19 @@ def move_joints( ) actual_duration = max(requested_duration, minimum_duration) self._check_actual_duration(actual_duration) - steps = max(2, math.ceil(actual_duration * self.config.control_rate_hz)) - interval = actual_duration / steps - feedback_stride = max( - 1, - round(self.config.control_rate_hz / self.config.feedback_rate_hz), - ) + control_interval = 1.0 / self.config.control_rate_hz + feedback_interval = 1.0 / self.config.feedback_rate_hz start_time = self._clock() + next_feedback = start_time + feedback_interval try: - for index in range(1, steps + 1): - self._raise_if_cancelled() - scale = _min_jerk(index / steps) + while True: + if self._sleep_interruptible(control_interval): + raise MotionCancelled("joint motion cancelled by stop request") + progress = min( + max((self._clock() - start_time) / actual_duration, 0.0), 1.0 + ) + scale = _min_jerk(progress) waypoint = [ initial + (goal - initial) * scale for initial, goal in zip(start, target) @@ -270,14 +274,14 @@ def move_joints( self._send_mit_locked(joint, position) with self._state_lock: self._last_targets = waypoint - - remaining = start_time + index * interval - self._clock() - if remaining > 0 and self._sleep_interruptible(remaining): - raise MotionCancelled("joint motion cancelled by stop request") - if index % feedback_stride == 0 or index == steps: + now = self._clock() + if now >= next_feedback or progress >= 1.0: self._validate_motion_feedback( self._sample_feedback(), waypoint ) + next_feedback = self._clock() + feedback_interval + if progress >= 1.0: + break settled = self._wait_for_joint_target(target) except MotionCancelled: @@ -323,7 +327,6 @@ def set_gripper( "gripper is not calibrated; configure open_position and closed_position" ) - self._cancel_event.clear() try: snapshot = self._sample_feedback() except Exception as exc: @@ -345,28 +348,30 @@ def set_gripper( actual_duration = max(requested_duration, minimum_duration) self._check_actual_duration(actual_duration) self._ensure_gripper_enabled(gripper, start) - steps = max(2, math.ceil(actual_duration * self.config.control_rate_hz)) - interval = actual_duration / steps - feedback_stride = max( - 1, - round(self.config.control_rate_hz / self.config.feedback_rate_hz), - ) + control_interval = 1.0 / self.config.control_rate_hz + feedback_interval = 1.0 / self.config.feedback_rate_hz start_time = self._clock() + next_feedback = start_time + feedback_interval try: - for index in range(1, steps + 1): - self._raise_if_cancelled() - waypoint = start + (target - start) * _min_jerk(index / steps) - with self._io_lock: - self._send_mit_locked(gripper, waypoint) - remaining = start_time + index * interval - self._clock() - if remaining > 0 and self._sleep_interruptible(remaining): + while True: + if self._sleep_interruptible(control_interval): raise MotionCancelled( "gripper motion cancelled by stop request" ) - if index % feedback_stride == 0 or index == steps: + progress = min( + max((self._clock() - start_time) / actual_duration, 0.0), 1.0 + ) + waypoint = start + (target - start) * _min_jerk(progress) + with self._io_lock: + self._send_mit_locked(gripper, waypoint) + now = self._clock() + if now >= next_feedback or progress >= 1.0: feedback = self._sample_feedback() self._validate_gripper_feedback(feedback, waypoint) + next_feedback = self._clock() + feedback_interval + if progress >= 1.0: + break settled = self._wait_for_gripper_target(target) except MotionCancelled: @@ -390,39 +395,63 @@ def set_gripper( } def stop_motion(self) -> dict[str, Any]: - """Preempt motion, latch a software stop, and hold fresh arm feedback.""" + """Preempt motion, latch a software stop, and hold fresh motor feedback.""" self._cancel_event.set() with self._state_lock: self._require_connected_locked() self._stopped = True enabled = self._enabled - if enabled: + gripper_enabled = self._gripper_enabled + if enabled or gripper_enabled: try: snapshot = self._sample_feedback() - self._validate_startup_positions(snapshot) - self._assert_no_faults( - snapshot["faults"], - {joint.motor_id for joint in self.config.joints}, - context="soft stop", - ) + if enabled: + self._validate_startup_positions(snapshot) + self._assert_no_faults( + snapshot["faults"], + {joint.motor_id for joint in self.config.joints}, + context="soft stop", + ) + if gripper_enabled: + self._assert_no_faults( + snapshot["faults"], + {self.config.gripper.motor_id}, + context="gripper soft stop", + ) with self._io_lock: - for joint in self.config.joints: + if enabled: + for joint in self.config.joints: + self._send_mit_locked( + joint, snapshot["positions"][joint.motor_id] + ) + if gripper_enabled: + gripper = self.config.gripper self._send_mit_locked( - joint, snapshot["positions"][joint.motor_id] + gripper, snapshot["positions"][gripper.motor_id] ) with self._state_lock: - self._last_targets = [ - snapshot["positions"][joint.motor_id] - for joint in self.config.joints - ] + if enabled: + self._last_targets = [ + snapshot["positions"][joint.motor_id] + for joint in self.config.joints + ] except Exception as exc: self._fail_closed(exc) - return {"enabled": self.enabled, "stopped": True} + with self._state_lock: + return { + "enabled": self._enabled, + "gripper_enabled": self._gripper_enabled, + "stopped": True, + } def reset_stop(self) -> dict[str, Any]: """Clear only the software-stop latch; it never enables motors.""" with self._state_lock: self._require_connected_locked() + if self._disable_failed: + raise RuntimeError( + "disable_all previously failed; retry emergency_stop before reset_stop" + ) self._stopped = False self._cancel_event.clear() return {"enabled": self._enabled, "stopped": False} @@ -475,6 +504,7 @@ def close(self) -> None: self._enabled = False self._gripper_enabled = False self._stopped = True + self._disable_failed = False self._previous_positions = None self._previous_sample_time = None @@ -741,6 +771,9 @@ def _check_actual_duration(self, duration_s: float) -> None: ) def _disable_after_timeout(self, reason: str) -> None: + self._cancel_event.set() + with self._state_lock: + self._stopped = True try: self._disable_all() except Exception as exc: @@ -759,15 +792,22 @@ def _fail_closed(self, error: Exception) -> NoReturn: raise error def _disable_all(self) -> None: - with self._io_lock: - controller = self._controller - if controller is None: - return - controller.disable_all() + try: + with self._io_lock: + controller = self._controller + if controller is None: + return + controller.disable_all() + except Exception: + with self._state_lock: + self._stopped = True + self._disable_failed = True + raise with self._state_lock: self._enabled = False self._gripper_enabled = False self._stopped = True + self._disable_failed = False def _validate_joint_limits(self, target: Sequence[float]) -> None: for joint, value in zip(self.config.joints, target): diff --git a/robots/rebot_robstride/toolkit.py b/robots/rebot_robstride/toolkit.py index 28438f59..16a2d418 100644 --- a/robots/rebot_robstride/toolkit.py +++ b/robots/rebot_robstride/toolkit.py @@ -10,9 +10,9 @@ { "name": "get_robot_state", "description": ( - "Read fresh RobStride joint, velocity, gripper, enable, and stop state. " - "Includes raw fault/warning reports. Call this before enabling or planning " - "any motion." + "Read fresh RobStride joint, velocity, gripper, enable, stop, and " + "disable-failure state. Includes raw fault/warning reports. Call this " + "before enabling or planning any motion." ), "input_schema": {"type": "object", "properties": {}, "required": []}, }, diff --git a/tests/robots/rebot_robstride/test_config.py b/tests/robots/rebot_robstride/test_config.py index e609ff13..e0563b56 100644 --- a/tests/robots/rebot_robstride/test_config.py +++ b/tests/robots/rebot_robstride/test_config.py @@ -93,3 +93,25 @@ def test_load_config_rejects_non_numeric_nested_value(tmp_path: Path) -> None: with pytest.raises(ValueError, match="gripper contains a non-finite value"): load_config(path) + + +@pytest.mark.parametrize( + ("yaml_text", "message"), + [ + ("control_rate_hz: 1\n", "control_rate_hz must be in"), + ("feedback_rate_hz: 0.0001\n", "feedback_rate_hz must be in"), + ("settle_timeout_s: 100000\n", "settle_timeout_s must not exceed"), + ("heartbeat_timeout_s: 100000\n", "heartbeat_timeout_s must not exceed"), + ("max_motion_duration_s: 100\n", "max_motion_duration_s must not exceed"), + ("gripper: {kp: 1000000000}\n", "gripper gains must satisfy"), + ("gripper: {max_velocity: 1000000000}\n", "gripper max_velocity"), + ], +) +def test_load_config_rejects_values_above_safety_ceilings( + tmp_path: Path, yaml_text: str, message: str +) -> None: + path = tmp_path / "unsafe.yaml" + path.write_text(yaml_text) + + with pytest.raises(ValueError, match=message): + load_config(path) diff --git a/tests/robots/rebot_robstride/test_driver.py b/tests/robots/rebot_robstride/test_driver.py index 48afb65f..b5b578f6 100644 --- a/tests/robots/rebot_robstride/test_driver.py +++ b/tests/robots/rebot_robstride/test_driver.py @@ -52,15 +52,24 @@ def __init__( self.block_send = False self.send_entered = threading.Event() self.release_send = threading.Event() + self.read_delay_s = 0.0 + + def _delay_read(self) -> None: + if self.read_delay_s <= 0: + return + sleep = getattr(self.clock, "sleep", time.sleep) + sleep(self.read_delay_s) def robstride_get_param_f32(self, parameter: int, timeout_ms: int = 1000) -> float: self.calls.append(("read", parameter, timeout_ms)) + self._delay_read() if parameter != MECH_POS: raise AssertionError(f"unexpected parameter {parameter:#x}") return self.position def robstride_get_fault_report(self) -> tuple[int, int]: self.calls.append(("fault_report",)) + self._delay_read() return self.fault_raw, self.warning_raw def clear_error(self) -> None: @@ -110,6 +119,7 @@ def __init__( self.clock = clock self.motors: dict[int, FakeMotor] = {} self.disabled = False + self.fail_disable = False self.closed = False @property @@ -130,6 +140,8 @@ def add_robstride_motor( return motor def disable_all(self) -> None: + if self.fail_disable: + raise RuntimeError("injected disable failure") self.disabled = True for motor in self.motors.values(): motor.enabled = False @@ -342,6 +354,29 @@ def test_minimum_jerk_profile_respects_configured_velocity_cap() -> None: assert result["actual_duration_s"] >= 1.875 +def test_feedback_latency_does_not_create_catch_up_bursts() -> None: + driver, controllers, _ = make_driver() + driver.connect() + driver.enable() + for motor in controllers[0].motors.values(): + motor.read_delay_s = 0.003 + motor = controllers[0].motors[1] + command_count = sum(call[0] == "send_mit" for call in motor.calls) + + driver.move_joints([0.5, 0.0, 0.0, 0.0, 0.0, 0.0], duration_s=0.1) + + commands = [call for call in motor.calls if call[0] == "send_mit"][command_count:] + intervals = [ + current[6] - previous[6] for previous, current in zip(commands, commands[1:]) + ] + velocities = [ + abs(current[1] - previous[1]) / interval + for previous, current, interval in zip(commands, commands[1:], intervals) + ] + assert min(intervals) >= 1.0 / driver.config.control_rate_hz - 1e-9 + assert max(velocities) <= driver.config.joints[0].max_velocity * 1.02 + + def test_motion_transport_failure_disables_all_and_latches_stop() -> None: driver, controllers, _ = make_driver() driver.connect() @@ -401,6 +436,33 @@ def test_settlement_timeout_returns_unreached_and_disables() -> None: assert controllers[0].disabled +def test_settlement_disable_failure_latches_stop_and_blocks_reset() -> None: + config = replace( + default_config(), + max_tracking_error_rad=0.5, + settle_timeout_s=0.2, + ) + driver, controllers, _ = make_driver(config=config) + driver.connect() + driver.enable() + controllers[0].motors[1].follow_commands = False + controllers[0].fail_disable = True + + with pytest.raises(RuntimeError, match="disable_all also failed"): + driver.move_joints([0.1, 0.0, 0.0, 0.0, 0.0, 0.0], duration_s=0.2) + + state = driver.state() + assert state["enabled"] is True + assert state["stopped"] is True + assert state["disable_failed"] is True + with pytest.raises(RuntimeError, match="retry emergency_stop"): + driver.reset_stop() + + controllers[0].fail_disable = False + driver.emergency_stop() + assert driver.state()["disable_failed"] is False + + def test_move_joints_rejects_limit_and_duration_violations() -> None: driver, _, _ = make_driver() driver.connect() @@ -532,6 +594,68 @@ def test_gripper_maps_and_returns_settlement_evidence() -> None: assert controllers[0].motors[7].enabled +def test_estop_between_gripper_ready_and_feedback_cannot_reenable() -> None: + config = default_config() + calibrated = replace( + config, + gripper=replace(config.gripper, open_position=-1.0, closed_position=0.0), + ) + driver, controllers, _ = make_driver(config=calibrated) + driver.connect() + driver.enable() + original_sample = driver._sample_feedback + sample_entered = threading.Event() + release_sample = threading.Event() + motion_error: list[BaseException] = [] + + def blocked_sample(): + sample_entered.set() + if not release_sample.wait(timeout=2.0): + raise TimeoutError("test did not release feedback") + return original_sample() + + def move_gripper() -> None: + try: + driver.set_gripper(0.5) + except BaseException as exc: # captured for the test thread + motion_error.append(exc) + + driver._sample_feedback = blocked_sample + thread = threading.Thread(target=move_gripper) + thread.start() + assert sample_entered.wait(timeout=1.0) + + driver.emergency_stop() + release_sample.set() + thread.join(timeout=1.0) + + assert not thread.is_alive() + assert motion_error and isinstance(motion_error[0], MotionCancelled) + assert not controllers[0].motors[7].enabled + assert driver.state()["enabled"] is False + + +def test_soft_stop_holds_enabled_gripper_at_measured_position() -> None: + config = default_config() + calibrated = replace( + config, + gripper=replace(config.gripper, open_position=-1.0, closed_position=0.0), + ) + driver, controllers, _ = make_driver(config=calibrated) + driver.connect() + driver.enable() + driver.set_gripper(0.5) + gripper = controllers[0].motors[7] + gripper.position = -0.4 + + result = driver.stop_motion() + + gripper_commands = [call for call in gripper.calls if call[0] == "send_mit"] + assert gripper_commands[-1][1] == pytest.approx(-0.4) + assert result["gripper_enabled"] is True + assert result["stopped"] is True + + def test_gripper_transport_failure_disables_all() -> None: config = default_config() calibrated = replace( From f515f9f8d66dad62ec705cdcb804944c9c046ef8 Mon Sep 17 00:00:00 2001 From: johnnynunez Date: Mon, 20 Jul 2026 06:58:34 +0200 Subject: [PATCH 5/6] fix(rebot): linearize safety state transitions --- .../rst_source/usage/rebot_robstride.rst | 74 ++- robots/rebot_robstride/config.py | 7 +- robots/rebot_robstride/driver.py | 467 +++++++++++++----- robots/rebot_robstride/env_client.py | 9 +- scripts/check_rebot_robstride.py | 5 +- tests/robots/rebot_robstride/test_config.py | 1 + tests/robots/rebot_robstride/test_driver.py | 348 ++++++++++++- .../robots/rebot_robstride/test_env_client.py | 6 + .../robots/rebot_robstride/test_env_server.py | 13 + 9 files changed, 769 insertions(+), 161 deletions(-) diff --git a/docs/source-en/rst_source/usage/rebot_robstride.rst b/docs/source-en/rst_source/usage/rebot_robstride.rst index 74c0e0a4..78fa339e 100644 --- a/docs/source-en/rst_source/usage/rebot_robstride.rst +++ b/docs/source-en/rst_source/usage/rebot_robstride.rst @@ -36,11 +36,12 @@ Verify all seven motors before enabling torque: python scripts/check_rebot_robstride.py -The check only reads RobStride ``mechPos`` (``0x7019``) and fault/warning -reports. It never clears faults, changes modes, enables motors, or sends a -target. Runtime velocity feedback is estimated from timestamped position -samples because ``mechVel`` scaling is not consistent across the tested -RobStride firmware variants. +The check enables RobStride active fault reporting, reads ``mechPos`` +(``0x7019``), and inspects status plus fault/warning reports. It never clears +faults, selects a control mode, enables torque, or sends a target. On exit it +still issues ``disable_all`` before releasing SocketCAN. Runtime velocity +feedback is estimated from timestamped position samples because ``mechVel`` +scaling is not consistent across the tested RobStride firmware variants. Configuration ------------- @@ -74,11 +75,14 @@ null. Calibrate the installed gripper before setting them. RPent refuses every gripper command while either endpoint is missing. Safety-critical overrides have hard ceilings: control is 10–200 Hz, feedback -is at least 5 Hz, motion is at most 60 seconds, settlement is at most 5 seconds, -and their combined budget is at most 65 seconds versus the 75-second motion RPC -timeout. Heartbeat timeout is 0.25–5 seconds, joint/gripper velocity is at most -1 rad/s, and MIT gains are capped at ``kp=200`` / ``kd=20``. Configuration that -exceeds these bounds is rejected before opening SocketCAN. +is at least 5 Hz, each parameter-read timeout is at most 100 ms, motion is at +most 60 seconds, and settlement is at most 5 seconds. The nominal +motion-plus-settlement budget is at most 65 seconds. All initial/final feedback +overhead counts against a separate 68-second server deadline, which leaves +seven seconds before the 75-second motion RPC timeout for fail-closed disable +and response delivery. Heartbeat timeout is 0.25–5 seconds, joint/gripper +velocity is at most 1 rad/s, and MIT gains are capped at ``kp=200`` / ``kd=20``. +Configuration that exceeds these bounds is rejected before opening SocketCAN. Run --- @@ -96,16 +100,19 @@ Start a physical-agent run with an explicit natural-language instruction: The arm starts disabled. An agent must call ``get_robot_state`` and then ``enable_arm`` before any motion. ``enable_arm`` first validates raw joint -limits and near-zero startup velocity, clears faults, verifies they are clear, -selects MIT mode, enables only the six arm motors, and immediately holds the -observed pose. The gripper remains disabled until a calibrated gripper command +limits and near-zero startup velocity, clears faults, verifies the resulting +operation-status replies and cached detailed reports, selects MIT mode, enables +only the six arm motors, and immediately holds the observed pose. Repeated +``enable_arm`` calls are idempotent and do not discard an enabled gripper's +state. The gripper remains disabled until a calibrated gripper command explicitly enables it. Available robot tools --------------------- -- ``get_robot_state`` — fresh positions, estimated velocities, and raw - fault/warning reports for all motors. +- ``get_robot_state`` — fresh positions, estimated velocities, latest available + operation-status flags, and actively reported raw fault/warning cache for all + motors. - ``enable_arm`` — explicit fault-clear, mode selection, enable, and pose hold. - ``move_joints`` — six raw-motor-radian targets through a bounded minimum-jerk trajectory with final read-back evidence. @@ -122,17 +129,28 @@ Safety limits limits, malformed six-joint vectors, and durations above the configured hard maximum. Requested durations are stretched by the 1.875 peak derivative of the minimum-jerk profile so the actual setpoint velocity respects each configured -cap. During motion the driver polls position and fault feedback, aborting and -disabling all motors on a transport error, fault/warning report, excessive -tracking error, or excessive measured velocity. Completion requires consecutive +cap. During motion the driver sends a hold or waypoint to every physically +enabled motor before feedback, including motors in the subsystem that is not +moving. Those commands produce operation-status replies; active reporting +updates the detailed fault/warning cache. The driver aborts and disables all +motors on missing/nonzero operation status, a nonzero raw fault/warning report, +transport error, excessive tracking error, excessive measured velocity, or the +68-second server deadline. ``motorbridge`` 0.4.9 does not expose a timestamp for +its detailed type-21 fault-report cache, so that raw cache is not described as a +synchronously queried sample. Completion requires consecutive position-and-velocity settlement samples. Gripper results use the same ``target``/``final``/``max_error``/``reached`` evidence contract. ``robot.stop_motion``, ``robot.emergency_stop``, heartbeats, and ``shutdown`` use a priority RPC path that bypasses the serialized motion-command lock. -Trajectories check a cancellation latch at every control tick. A runtime worker -sends periodic heartbeats; if the agent process disappears while torque is -enabled, the server calls emergency stop after ``heartbeat_timeout_s``. +Every admitted operation captures a monotonic stop generation; a stop +invalidates that generation permanently, so a later ``reset_stop`` cannot +resurrect an older enable or trajectory. Feedback and multi-joint command +batches release the I/O lock between motor operations, so an emergency stop +waits behind at most one in-flight motorbridge call before disable begins. A +runtime worker sends periodic heartbeats; if the agent process disappears while +torque is enabled, the server calls emergency stop after +``heartbeat_timeout_s``. Soft stop holds both the arm and an enabled gripper at freshly measured positions. If ``disable_all`` itself fails, state reports ``disable_failed=true`` and ``reset_stop`` remains blocked until a later emergency-stop retry confirms @@ -145,12 +163,14 @@ it through a TCP proxy or bind it to a LAN address. Support the arm and keep the physical emergency stop accessible during initial hardware validation. -Normal shutdown always disables enabled motors; there is no unattended ``hold`` -shutdown mode. The heartbeat protects loss of the agent process while the -hardware server remains alive. A hard crash or ``SIGKILL`` of the hardware -server cannot be proven fail-safe because the tested RobStride/motorbridge API -does not expose a confirmed hardware watchdog. Never operate unattended, and -keep the physical emergency stop reachable. +Normal shutdown always calls ``disable_all``; there is no unattended ``hold`` +shutdown mode. If disable fails, ``close`` does not close the controller or erase +the physically uncertain enabled/``disable_failed`` state, allowing a later +emergency-stop or close retry. The heartbeat protects loss of the agent process +while the hardware server remains alive. A hard crash or ``SIGKILL`` of the +hardware server cannot be proven fail-safe because the tested +RobStride/motorbridge API does not expose a confirmed hardware watchdog. Never +operate unattended, and keep the physical emergency stop reachable. The first implementation provides guarded joint-space control. It does not claim collision avoidance, Cartesian planning, or perception-based grasping. diff --git a/robots/rebot_robstride/config.py b/robots/rebot_robstride/config.py index cc4419f3..c701d839 100644 --- a/robots/rebot_robstride/config.py +++ b/robots/rebot_robstride/config.py @@ -7,6 +7,9 @@ from pathlib import Path from typing import Any +MOTION_RPC_TIMEOUT_S = 75.0 +SERVER_MOTION_DEADLINE_S = 68.0 + @dataclass(frozen=True) class JointConfig: @@ -180,9 +183,9 @@ def _validate(config: RebotConfig) -> RebotConfig: if ( isinstance(config.read_timeout_ms, bool) or not isinstance(config.read_timeout_ms, int) - or not 1 <= config.read_timeout_ms <= 1000 + or not 1 <= config.read_timeout_ms <= 100 ): - raise ValueError("read_timeout_ms must be an integer in [1, 1000]") + raise ValueError("read_timeout_ms must be an integer in [1, 100]") finite_positive = { "control_rate_hz": config.control_rate_hz, diff --git a/robots/rebot_robstride/driver.py b/robots/rebot_robstride/driver.py index 55d5d8d8..94fc0ef2 100644 --- a/robots/rebot_robstride/driver.py +++ b/robots/rebot_robstride/driver.py @@ -8,7 +8,12 @@ from collections.abc import Callable, Sequence from typing import Any, NoReturn -from robots.rebot_robstride.config import GripperConfig, JointConfig, RebotConfig +from robots.rebot_robstride.config import ( + SERVER_MOTION_DEADLINE_S, + GripperConfig, + JointConfig, + RebotConfig, +) MECH_POS = 0x7019 ROBSTRIDE_HOST_ID = 0xFD @@ -63,7 +68,10 @@ def __init__( self._gripper_enabled = False self._stopped = False self._disable_failed = False + self._stop_epoch = 0 + self._active_stops = 0 self._last_targets: list[float] | None = None + self._last_gripper_target: float | None = None self._previous_positions: dict[int, float] | None = None self._previous_sample_time: float | None = None self._last_heartbeat = self._clock() @@ -105,6 +113,8 @@ def connect(self) -> dict[str, Any]: ) for motor in motor_configs } + for motor in motors.values(): + motor.robstride_set_active_report(True) with self._state_lock: self._controller = controller self._motors = motors @@ -123,8 +133,12 @@ def connect(self) -> dict[str, Any]: def state(self) -> dict[str, Any]: """Return fresh positions, estimated velocities, and fault reports.""" - snapshot = self._sample_feedback() with self._state_lock: + self._require_connected_locked() + operation_epoch = self._stop_epoch + snapshot = self._sample_feedback(operation_epoch) + with self._state_lock: + self._assert_epoch_current(operation_epoch) enabled = self._enabled gripper_enabled = self._gripper_enabled stopped = self._stopped @@ -154,17 +168,30 @@ def state(self) -> dict[str, Any]: def enable(self) -> dict[str, Any]: """Validate startup state, enable arm joints, and hold observed pose.""" with self._motion_lock: - self._require_connected() with self._state_lock: + self._require_connected_locked() + if self._active_stops: + raise RuntimeError("stop is in progress") if self._stopped: raise RuntimeError( "arm is stopped; call reset_stop before enabling" ) - self._cancel_event.clear() - first = self._sample_feedback() + if self._cancel_event.is_set(): + raise RuntimeError("arm cancellation is latched; call reset_stop") + operation_epoch = self._stop_epoch + already_enabled = self._enabled + if already_enabled: + snapshot = self.state() + return { + "enabled": True, + "gripper_enabled": snapshot["gripper_enabled"], + "hold_positions": list(snapshot["joint_positions"]), + } + first = self._sample_feedback(operation_epoch) self._validate_startup_positions(first) self._sleep(self.config.startup_sample_interval_s) - second = self._sample_feedback() + self._assert_operation_active(operation_epoch) + second = self._sample_feedback(operation_epoch) self._validate_startup_positions(second) self._validate_startup_velocity(second) @@ -172,11 +199,11 @@ def enable(self) -> dict[str, Any]: try: for motor_config in arm_configs: with self._io_lock: - self._raise_if_cancelled() + self._assert_operation_active(operation_epoch) motor = self._motors[motor_config.motor_id] motor.clear_error() with self._io_lock: - self._raise_if_cancelled() + self._assert_operation_active(operation_epoch) self._assert_no_faults( self._read_faults_locked(), {joint.motor_id for joint in arm_configs}, @@ -184,17 +211,17 @@ def enable(self) -> dict[str, Any]: ) for motor_config in arm_configs: with self._io_lock: - self._raise_if_cancelled() + self._assert_operation_active(operation_epoch) self._motors[motor_config.motor_id].ensure_mode( MIT_MODE, timeout_ms=1000 ) for motor_config in arm_configs: with self._io_lock: - self._raise_if_cancelled() + self._assert_operation_active(operation_epoch) self._motors[motor_config.motor_id].enable() for motor_config in arm_configs: with self._io_lock: - self._raise_if_cancelled() + self._assert_operation_active(operation_epoch) self._send_mit_locked( motor_config, second["positions"][motor_config.motor_id], @@ -204,10 +231,8 @@ def enable(self) -> dict[str, Any]: try: with self._io_lock, self._state_lock: - self._raise_if_cancelled() + self._assert_operation_active(operation_epoch) self._enabled = True - self._gripper_enabled = False - self._stopped = False self._last_heartbeat = self._clock() self._last_targets = [ second["positions"][joint.motor_id] for joint in arm_configs @@ -216,7 +241,7 @@ def enable(self) -> dict[str, Any]: self._fail_closed(exc) return { "enabled": True, - "gripper_enabled": False, + "gripper_enabled": self._gripper_enabled, "hold_positions": list(self._last_targets), } @@ -228,7 +253,8 @@ def move_joints( ) -> dict[str, Any]: """Execute a monitored minimum-jerk trajectory and verify settlement.""" with self._motion_lock: - self._require_motion_ready() + operation_epoch = self._require_motion_ready() + server_deadline = self._clock() + SERVER_MOTION_DEADLINE_S target = [float(value) for value in positions] if len(target) != len(self.config.joints): raise ValueError("positions must contain exactly six joint values") @@ -238,7 +264,8 @@ def move_joints( requested_duration = self._validate_duration(duration_s) try: - start_state = self._sample_feedback() + self._send_enabled_holds(operation_epoch, server_deadline) + start_state = self._sample_feedback(operation_epoch, server_deadline) start = [ start_state["positions"][joint.motor_id] for joint in self.config.joints @@ -259,7 +286,9 @@ def move_joints( try: while True: - if self._sleep_interruptible(control_interval): + if self._sleep_interruptible( + control_interval, operation_epoch, server_deadline + ): raise MotionCancelled("joint motion cancelled by stop request") progress = min( max((self._clock() - start_time) / actual_duration, 0.0), 1.0 @@ -269,21 +298,22 @@ def move_joints( initial + (goal - initial) * scale for initial, goal in zip(start, target) ] - with self._io_lock: - for joint, position in zip(self.config.joints, waypoint): - self._send_mit_locked(joint, position) - with self._state_lock: - self._last_targets = waypoint + self._send_enabled_holds( + operation_epoch, server_deadline, arm_targets=waypoint + ) now = self._clock() if now >= next_feedback or progress >= 1.0: self._validate_motion_feedback( - self._sample_feedback(), waypoint + self._sample_feedback(operation_epoch, server_deadline), + waypoint, ) next_feedback = self._clock() + feedback_interval if progress >= 1.0: break - settled = self._wait_for_joint_target(target) + settled = self._wait_for_joint_target( + target, operation_epoch, server_deadline + ) except MotionCancelled: raise except Exception as exc: @@ -316,7 +346,8 @@ def set_gripper( ) -> dict[str, Any]: """Enable and move the calibrated gripper; 0.0=open and 1.0=closed.""" with self._motion_lock: - self._require_motion_ready() + operation_epoch = self._require_motion_ready() + server_deadline = self._clock() + SERVER_MOTION_DEADLINE_S normalized = float(position) if not math.isfinite(normalized) or not 0.0 <= normalized <= 1.0: raise ValueError("gripper position must be in [0.0, 1.0]") @@ -328,7 +359,8 @@ def set_gripper( ) try: - snapshot = self._sample_feedback() + self._send_enabled_holds(operation_epoch, server_deadline) + snapshot = self._sample_feedback(operation_epoch, server_deadline) except Exception as exc: self._fail_closed(exc) start = snapshot["positions"][gripper.motor_id] @@ -347,7 +379,9 @@ def set_gripper( ) actual_duration = max(requested_duration, minimum_duration) self._check_actual_duration(actual_duration) - self._ensure_gripper_enabled(gripper, start) + self._ensure_gripper_enabled( + gripper, start, operation_epoch, server_deadline + ) control_interval = 1.0 / self.config.control_rate_hz feedback_interval = 1.0 / self.config.feedback_rate_hz start_time = self._clock() @@ -355,7 +389,9 @@ def set_gripper( try: while True: - if self._sleep_interruptible(control_interval): + if self._sleep_interruptible( + control_interval, operation_epoch, server_deadline + ): raise MotionCancelled( "gripper motion cancelled by stop request" ) @@ -363,17 +399,24 @@ def set_gripper( max((self._clock() - start_time) / actual_duration, 0.0), 1.0 ) waypoint = start + (target - start) * _min_jerk(progress) - with self._io_lock: - self._send_mit_locked(gripper, waypoint) + self._send_enabled_holds( + operation_epoch, + server_deadline, + gripper_target=waypoint, + ) now = self._clock() if now >= next_feedback or progress >= 1.0: - feedback = self._sample_feedback() + feedback = self._sample_feedback( + operation_epoch, server_deadline + ) self._validate_gripper_feedback(feedback, waypoint) next_feedback = self._clock() + feedback_interval if progress >= 1.0: break - settled = self._wait_for_gripper_target(target) + settled = self._wait_for_gripper_target( + target, operation_epoch, server_deadline + ) except MotionCancelled: raise except Exception as exc: @@ -396,15 +439,13 @@ def set_gripper( def stop_motion(self) -> dict[str, Any]: """Preempt motion, latch a software stop, and hold fresh motor feedback.""" - self._cancel_event.set() - with self._state_lock: - self._require_connected_locked() - self._stopped = True - enabled = self._enabled - gripper_enabled = self._gripper_enabled - if enabled or gripper_enabled: - try: - snapshot = self._sample_feedback() + stop_epoch = self._begin_stop() + try: + with self._state_lock: + enabled = self._enabled + gripper_enabled = self._gripper_enabled + if enabled or gripper_enabled: + snapshot = self._sample_feedback(stop_epoch) if enabled: self._validate_startup_positions(snapshot) self._assert_no_faults( @@ -418,14 +459,17 @@ def stop_motion(self) -> dict[str, Any]: {self.config.gripper.motor_id}, context="gripper soft stop", ) - with self._io_lock: - if enabled: - for joint in self.config.joints: + if enabled: + for joint in self.config.joints: + with self._io_lock: + self._assert_epoch_current(stop_epoch) self._send_mit_locked( joint, snapshot["positions"][joint.motor_id] ) - if gripper_enabled: - gripper = self.config.gripper + if gripper_enabled: + gripper = self.config.gripper + with self._io_lock: + self._assert_epoch_current(stop_epoch) self._send_mit_locked( gripper, snapshot["positions"][gripper.motor_id] ) @@ -435,19 +479,27 @@ def stop_motion(self) -> dict[str, Any]: snapshot["positions"][joint.motor_id] for joint in self.config.joints ] - except Exception as exc: - self._fail_closed(exc) - with self._state_lock: - return { - "enabled": self._enabled, - "gripper_enabled": self._gripper_enabled, - "stopped": True, - } + if gripper_enabled: + self._last_gripper_target = snapshot["positions"][ + self.config.gripper.motor_id + ] + with self._state_lock: + return { + "enabled": self._enabled, + "gripper_enabled": self._gripper_enabled, + "stopped": True, + } + except Exception as exc: + self._fail_closed(exc) + finally: + self._end_stop() def reset_stop(self) -> dict[str, Any]: """Clear only the software-stop latch; it never enables motors.""" with self._state_lock: self._require_connected_locked() + if self._active_stops: + raise RuntimeError("stop is in progress") if self._disable_failed: raise RuntimeError( "disable_all previously failed; retry emergency_stop before reset_stop" @@ -458,12 +510,12 @@ def reset_stop(self) -> dict[str, Any]: def emergency_stop(self) -> dict[str, Any]: """Preempt motion and disable every motor without waiting for trajectory locks.""" - self._cancel_event.set() - with self._state_lock: - self._require_connected_locked() - self._stopped = True - self._disable_all() - return {"enabled": False, "stopped": True} + self._begin_stop() + try: + self._disable_all() + return {"enabled": False, "stopped": True} + finally: + self._end_stop() def heartbeat(self) -> dict[str, Any]: """Refresh the agent-process deadman without changing motor state.""" @@ -484,18 +536,18 @@ def enforce_heartbeat_deadman(self) -> bool: return True def close(self) -> None: - """Cancel motion, disable enabled motors, and release the controller.""" - self._cancel_event.set() - with self._motion_lock: - with self._state_lock: - controller = self._controller - should_disable = self._enabled or self._gripper_enabled - if controller is None: - return - try: - if should_disable: - self._disable_all() - finally: + """Cancel motion, confirm all motors disabled, then release the controller.""" + stop_epoch = self._begin_stop_if_connected() + if stop_epoch is None: + return + try: + with self._motion_lock: + with self._state_lock: + controller = self._controller + if controller is None: + return + + self._disable_all() with self._io_lock: controller.close() with self._state_lock: @@ -507,17 +559,29 @@ def close(self) -> None: self._disable_failed = False self._previous_positions = None self._previous_sample_time = None + self._last_gripper_target = None + finally: + self._end_stop() - def _sample_feedback(self) -> dict[str, Any]: + def _sample_feedback( + self, operation_epoch: int, server_deadline: float | None = None + ) -> dict[str, Any]: self._require_connected() - with self._io_lock: - positions = { - motor_id: self._read_position_locked(motor_id) - for motor_id in self._motors - } - faults = self._read_faults_locked() + positions: dict[int, float] = {} + for motor_id in self._motors: + with self._io_lock: + self._assert_epoch_current(operation_epoch, server_deadline) + positions[motor_id] = self._read_position_locked(motor_id) + + faults: dict[int, dict[str, int | bool]] = {} + for motor_id, motor in self._motors.items(): + with self._io_lock: + self._assert_epoch_current(operation_epoch, server_deadline) + faults[motor_id] = self._read_motor_fault_locked(motor) + timestamp = self._clock() with self._state_lock: + self._assert_epoch_current(operation_epoch, server_deadline) previous_positions = self._previous_positions previous_time = self._previous_sample_time if ( @@ -553,15 +617,22 @@ def _read_position_locked(self, motor_id: int) -> float: ) return position - def _read_faults_locked(self) -> dict[int, dict[str, int]]: - reports: dict[int, dict[str, int]] = {} - for motor_id, motor in self._motors.items(): - fault_raw, warning_raw = motor.robstride_get_fault_report() - reports[motor_id] = { - "fault_raw": int(fault_raw), - "warning_raw": int(warning_raw), - } - return reports + def _read_faults_locked(self) -> dict[int, dict[str, int | bool]]: + return { + motor_id: self._read_motor_fault_locked(motor) + for motor_id, motor in self._motors.items() + } + + @staticmethod + def _read_motor_fault_locked(motor: Any) -> dict[str, int | bool]: + status = motor.get_state() + fault_raw, warning_raw = motor.robstride_get_fault_report() + return { + "status_available": status is not None, + "status_code": int(status.status_code) if status is not None else 0, + "fault_raw": int(fault_raw), + "warning_raw": int(warning_raw), + } def _send_mit_locked( self, config: JointConfig | GripperConfig, position: float @@ -570,6 +641,50 @@ def _send_mit_locked( float(position), 0.0, float(config.kp), float(config.kd), 0.0 ) + def _send_enabled_holds( + self, + operation_epoch: int, + server_deadline: float, + *, + arm_targets: Sequence[float] | None = None, + gripper_target: float | None = None, + ) -> None: + with self._state_lock: + arm_enabled = self._enabled + gripper_enabled = self._gripper_enabled + resolved_arm_targets = ( + list(arm_targets) if arm_targets is not None else self._last_targets + ) + resolved_gripper_target = ( + gripper_target + if gripper_target is not None + else self._last_gripper_target + ) + + if arm_enabled: + if resolved_arm_targets is None: + raise RuntimeError("enabled arm has no hold targets") + if len(resolved_arm_targets) != len(self.config.joints): + raise RuntimeError("enabled arm hold target count is invalid") + for joint, target in zip(self.config.joints, resolved_arm_targets): + with self._io_lock: + self._assert_operation_active(operation_epoch, server_deadline) + self._send_mit_locked(joint, target) + + if gripper_enabled: + if resolved_gripper_target is None: + raise RuntimeError("enabled gripper has no hold target") + with self._io_lock: + self._assert_operation_active(operation_epoch, server_deadline) + self._send_mit_locked(self.config.gripper, resolved_gripper_target) + + with self._state_lock: + self._assert_operation_active(operation_epoch, server_deadline) + if arm_targets is not None: + self._last_targets = list(arm_targets) + if gripper_target is not None: + self._last_gripper_target = gripper_target + def _validate_startup_positions(self, snapshot: dict[str, Any]) -> None: positions = [ snapshot["positions"][joint.motor_id] for joint in self.config.joints @@ -588,8 +703,9 @@ def _validate_startup_velocity(self, snapshot: dict[str, Any]) -> None: def _validate_motion_feedback( self, snapshot: dict[str, Any], waypoint: Sequence[float] ) -> None: - arm_ids = {joint.motor_id for joint in self.config.joints} - self._assert_no_faults(snapshot["faults"], arm_ids, context="motion") + self._assert_no_faults( + snapshot["faults"], self._enabled_motor_ids(), context="motion" + ) for joint, expected in zip(self.config.joints, waypoint): observed = snapshot["positions"][joint.motor_id] error = abs(observed - expected) @@ -611,7 +727,9 @@ def _validate_gripper_feedback( ) -> None: gripper = self.config.gripper self._assert_no_faults( - snapshot["faults"], {gripper.motor_id}, context="gripper motion" + snapshot["faults"], + self._enabled_motor_ids(), + context="gripper motion", ) error = abs(snapshot["positions"][gripper.motor_id] - waypoint) if error > self.config.max_tracking_error_rad: @@ -627,57 +745,95 @@ def _validate_gripper_feedback( f"abort limit {limit:.6f} rad/s" ) + def _enabled_motor_ids(self) -> set[int]: + with self._state_lock: + motor_ids = ( + {joint.motor_id for joint in self.config.joints} + if self._enabled + else set() + ) + if self._gripper_enabled: + motor_ids.add(self.config.gripper.motor_id) + return motor_ids + @staticmethod def _assert_no_faults( - reports: dict[int, dict[str, int]], motor_ids: set[int], *, context: str + reports: dict[int, dict[str, int | bool]], + motor_ids: set[int], + *, + context: str, ) -> None: + unavailable = [ + motor_id + for motor_id in sorted(motor_ids) + if not reports[motor_id]["status_available"] + ] + if unavailable: + raise RuntimeError( + f"missing RobStride operation status during {context} " + f"for motors {unavailable}" + ) active = { motor_id: reports[motor_id] for motor_id in sorted(motor_ids) - if reports[motor_id]["fault_raw"] or reports[motor_id]["warning_raw"] + if reports[motor_id]["status_code"] + or reports[motor_id]["fault_raw"] + or reports[motor_id]["warning_raw"] } if active: raise RuntimeError(f"RobStride fault during {context}: {active}") - def _ensure_gripper_enabled(self, gripper: GripperConfig, hold: float) -> None: + def _ensure_gripper_enabled( + self, + gripper: GripperConfig, + hold: float, + operation_epoch: int, + server_deadline: float, + ) -> None: with self._state_lock: if self._gripper_enabled: return try: motor = self._motors[gripper.motor_id] with self._io_lock: - self._raise_if_cancelled() + self._assert_operation_active(operation_epoch, server_deadline) motor.clear_error() with self._io_lock: - self._raise_if_cancelled() + self._assert_operation_active(operation_epoch, server_deadline) self._assert_no_faults( self._read_faults_locked(), - {gripper.motor_id}, + self._enabled_motor_ids() | {gripper.motor_id}, context="gripper startup", ) with self._io_lock: - self._raise_if_cancelled() + self._assert_operation_active(operation_epoch, server_deadline) motor.ensure_mode(MIT_MODE, timeout_ms=1000) with self._io_lock: - self._raise_if_cancelled() + self._assert_operation_active(operation_epoch, server_deadline) motor.enable() with self._io_lock: - self._raise_if_cancelled() + self._assert_operation_active(operation_epoch, server_deadline) self._send_mit_locked(gripper, hold) with self._io_lock, self._state_lock: - self._raise_if_cancelled() + self._assert_operation_active(operation_epoch, server_deadline) self._gripper_enabled = True + self._last_gripper_target = hold except Exception as exc: self._fail_closed(exc) - def _wait_for_joint_target(self, target: list[float]) -> dict[str, Any]: + def _wait_for_joint_target( + self, target: list[float], operation_epoch: int, server_deadline: float + ) -> dict[str, Any]: deadline = self._clock() + self.config.settle_timeout_s consecutive = 0 final_positions = target final_velocities = [math.inf] * len(target) while True: - self._raise_if_cancelled() - snapshot = self._sample_feedback() + self._assert_operation_active(operation_epoch, server_deadline) + self._send_enabled_holds( + operation_epoch, server_deadline, arm_targets=target + ) + snapshot = self._sample_feedback(operation_epoch, server_deadline) self._validate_motion_feedback(snapshot, target) final_positions = [ snapshot["positions"][joint.motor_id] for joint in self.config.joints @@ -705,18 +861,27 @@ def _wait_for_joint_target(self, target: list[float]) -> dict[str, Any]: "velocities": final_velocities, "reached": False, } - if self._sleep_interruptible(1.0 / self.config.feedback_rate_hz): + if self._sleep_interruptible( + 1.0 / self.config.feedback_rate_hz, + operation_epoch, + server_deadline, + ): raise MotionCancelled("joint settlement cancelled by stop request") - def _wait_for_gripper_target(self, target: float) -> dict[str, Any]: + def _wait_for_gripper_target( + self, target: float, operation_epoch: int, server_deadline: float + ) -> dict[str, Any]: deadline = self._clock() + self.config.settle_timeout_s consecutive = 0 motor_id = self.config.gripper.motor_id final_position = target final_velocity = math.inf while True: - self._raise_if_cancelled() - snapshot = self._sample_feedback() + self._assert_operation_active(operation_epoch, server_deadline) + self._send_enabled_holds( + operation_epoch, server_deadline, gripper_target=target + ) + snapshot = self._sample_feedback(operation_epoch, server_deadline) self._validate_gripper_feedback(snapshot, target) final_position = snapshot["positions"][motor_id] final_velocity = snapshot["velocities"][motor_id] @@ -737,22 +902,73 @@ def _wait_for_gripper_target(self, target: float) -> dict[str, Any]: "velocity": final_velocity, "reached": False, } - if self._sleep_interruptible(1.0 / self.config.feedback_rate_hz): + if self._sleep_interruptible( + 1.0 / self.config.feedback_rate_hz, + operation_epoch, + server_deadline, + ): raise MotionCancelled("gripper settlement cancelled by stop request") - def _sleep_interruptible(self, duration_s: float) -> bool: + def _sleep_interruptible( + self, + duration_s: float, + operation_epoch: int, + server_deadline: float | None = None, + ) -> bool: deadline = self._clock() + max(0.0, duration_s) while True: - if self._cancel_event.is_set(): - return True + self._assert_operation_active(operation_epoch, server_deadline) remaining = deadline - self._clock() if remaining <= 0: return False self._sleep(min(remaining, 0.01)) - def _raise_if_cancelled(self) -> None: - if self._cancel_event.is_set(): - raise MotionCancelled("motion cancelled by stop request") + def _begin_stop(self) -> int: + stop_epoch = self._begin_stop_if_connected() + if stop_epoch is None: + raise RuntimeError("arm is not connected") + return stop_epoch + + def _begin_stop_if_connected(self) -> int | None: + with self._state_lock: + if self._controller is None: + return None + self._stop_epoch += 1 + self._active_stops += 1 + self._stopped = True + self._cancel_event.set() + return self._stop_epoch + + def _end_stop(self) -> None: + with self._state_lock: + if self._active_stops <= 0: + raise RuntimeError("internal stop-operation counter underflow") + self._active_stops -= 1 + + def _assert_epoch_current( + self, operation_epoch: int, server_deadline: float | None = None + ) -> None: + with self._state_lock: + if operation_epoch != self._stop_epoch: + raise MotionCancelled("operation invalidated by stop request") + self._assert_server_deadline(server_deadline) + + def _assert_operation_active( + self, operation_epoch: int, server_deadline: float | None = None + ) -> None: + with self._state_lock: + if ( + operation_epoch != self._stop_epoch + or self._active_stops + or self._stopped + or self._cancel_event.is_set() + ): + raise MotionCancelled("motion cancelled by stop request") + self._assert_server_deadline(server_deadline) + + def _assert_server_deadline(self, server_deadline: float | None) -> None: + if server_deadline is not None and self._clock() >= server_deadline: + raise RuntimeError("server motion deadline exceeded") def _validate_duration(self, duration_s: float) -> float: duration = float(duration_s) @@ -771,24 +987,24 @@ def _check_actual_duration(self, duration_s: float) -> None: ) def _disable_after_timeout(self, reason: str) -> None: - self._cancel_event.set() - with self._state_lock: - self._stopped = True + self._begin_stop() try: self._disable_all() except Exception as exc: raise RuntimeError(f"{reason}; disable_all also failed: {exc}") from exc + finally: + self._end_stop() def _fail_closed(self, error: Exception) -> NoReturn: - self._cancel_event.set() - with self._state_lock: - self._stopped = True + self._begin_stop() try: self._disable_all() except Exception as disable_error: raise RuntimeError( f"{error}; fail-closed disable_all also failed: {disable_error}" ) from error + finally: + self._end_stop() raise error def _disable_all(self) -> None: @@ -825,10 +1041,15 @@ def _require_connected_locked(self) -> None: if self._controller is None: raise RuntimeError("arm is not connected") - def _require_motion_ready(self) -> None: + def _require_motion_ready(self) -> int: with self._state_lock: self._require_connected_locked() if not self._enabled: raise RuntimeError("arm is not enabled") + if self._active_stops: + raise RuntimeError("stop is in progress") if self._stopped: raise RuntimeError("arm is stopped; call reset_stop before moving") + if self._cancel_event.is_set(): + raise RuntimeError("arm cancellation is latched; call reset_stop") + return self._stop_epoch diff --git a/robots/rebot_robstride/env_client.py b/robots/rebot_robstride/env_client.py index 8e20b3dc..98f59f84 100644 --- a/robots/rebot_robstride/env_client.py +++ b/robots/rebot_robstride/env_client.py @@ -4,10 +4,11 @@ from typing import Any +from robots.rebot_robstride.config import MOTION_RPC_TIMEOUT_S from rpent.utils.rpc import RpcClient _DEFAULT_TIMEOUT_S = 10.0 -_MOTION_TIMEOUT_S = 75.0 +_ENABLE_TIMEOUT_S = 30.0 class RebotRobstrideEnvClient: @@ -20,7 +21,7 @@ def state(self) -> dict[str, Any]: return self._client.call("robot.state", timeout_s=_DEFAULT_TIMEOUT_S) def enable(self) -> dict[str, Any]: - return self._client.call("robot.enable", timeout_s=_DEFAULT_TIMEOUT_S) + return self._client.call("robot.enable", timeout_s=_ENABLE_TIMEOUT_S) def move_joints( self, positions: list[float], *, duration_s: float = 2.0 @@ -28,7 +29,7 @@ def move_joints( return self._client.call( "robot.move_joints", kwargs={"positions": positions, "duration_s": duration_s}, - timeout_s=_MOTION_TIMEOUT_S, + timeout_s=MOTION_RPC_TIMEOUT_S, ) def set_gripper( @@ -37,7 +38,7 @@ def set_gripper( return self._client.call( "robot.set_gripper", kwargs={"position": position, "duration_s": duration_s}, - timeout_s=_MOTION_TIMEOUT_S, + timeout_s=MOTION_RPC_TIMEOUT_S, ) def stop_motion(self) -> dict[str, Any]: diff --git a/scripts/check_rebot_robstride.py b/scripts/check_rebot_robstride.py index 74ce3e1f..24b79fe8 100644 --- a/scripts/check_rebot_robstride.py +++ b/scripts/check_rebot_robstride.py @@ -1,8 +1,9 @@ #!/usr/bin/env python3 """Passive seven-motor connectivity check for a reBot DevArm RobStride arm. -This script only reads RobStride mechPos and fault-report telemetry. It never -clears faults, changes modes, enables motors, or sends a target. +This script enables active fault reporting and reads RobStride mechPos plus +fault/status telemetry. It never clears faults, selects a control mode, enables +torque, or sends a target. On exit it confirms disable_all before releasing CAN. """ from __future__ import annotations diff --git a/tests/robots/rebot_robstride/test_config.py b/tests/robots/rebot_robstride/test_config.py index e0563b56..960a1042 100644 --- a/tests/robots/rebot_robstride/test_config.py +++ b/tests/robots/rebot_robstride/test_config.py @@ -103,6 +103,7 @@ def test_load_config_rejects_non_numeric_nested_value(tmp_path: Path) -> None: ("settle_timeout_s: 100000\n", "settle_timeout_s must not exceed"), ("heartbeat_timeout_s: 100000\n", "heartbeat_timeout_s must not exceed"), ("max_motion_duration_s: 100\n", "max_motion_duration_s must not exceed"), + ("read_timeout_ms: 101\n", "read_timeout_ms must be an integer"), ("gripper: {kp: 1000000000}\n", "gripper gains must satisfy"), ("gripper: {max_velocity: 1000000000}\n", "gripper max_velocity"), ], diff --git a/tests/robots/rebot_robstride/test_driver.py b/tests/robots/rebot_robstride/test_driver.py index b5b578f6..d3c2936b 100644 --- a/tests/robots/rebot_robstride/test_driver.py +++ b/tests/robots/rebot_robstride/test_driver.py @@ -3,6 +3,7 @@ import threading import time from dataclasses import replace +from types import SimpleNamespace import pytest @@ -26,6 +27,31 @@ def sleep(self, duration: float) -> None: self.now += max(0.0, duration) +class BlockingSetEvent: + def __init__(self) -> None: + self._event = threading.Event() + self.block_next_set = False + self.set_entered = threading.Event() + self.release_set = threading.Event() + + def set(self) -> None: + self._event.set() + if self.block_next_set: + self.block_next_set = False + self.set_entered.set() + if not self.release_set.wait(timeout=2.0): + raise TimeoutError("test did not release cancellation set") + + def clear(self) -> None: + self._event.clear() + + def is_set(self) -> bool: + return self._event.is_set() + + def wait(self, timeout: float | None = None) -> bool: + return self._event.wait(timeout) + + class FakeMotor: def __init__( self, @@ -53,8 +79,13 @@ def __init__( self.send_entered = threading.Event() self.release_send = threading.Event() self.read_delay_s = 0.0 + self.read_entered = threading.Event() + self.status_available = True + self.status_code = 0 + self.active_report = False def _delay_read(self) -> None: + self.read_entered.set() if self.read_delay_s <= 0: return sleep = getattr(self.clock, "sleep", time.sleep) @@ -72,6 +103,16 @@ def robstride_get_fault_report(self) -> tuple[int, int]: self._delay_read() return self.fault_raw, self.warning_raw + def get_state(self): + self.calls.append(("get_state",)) + if not self.status_available: + return None + return SimpleNamespace(status_code=self.status_code) + + def robstride_set_active_report(self, enabled: bool) -> None: + self.calls.append(("active_report", enabled)) + self.active_report = enabled + def clear_error(self) -> None: self.calls.append(("clear_error",)) if self.clear_fault_on_clear: @@ -120,6 +161,9 @@ def __init__( self.motors: dict[int, FakeMotor] = {} self.disabled = False self.fail_disable = False + self.block_disable = False + self.disable_entered = threading.Event() + self.release_disable = threading.Event() self.closed = False @property @@ -142,6 +186,10 @@ def add_robstride_motor( def disable_all(self) -> None: if self.fail_disable: raise RuntimeError("injected disable failure") + if self.block_disable: + self.disable_entered.set() + if not self.release_disable.wait(timeout=2.0): + raise TimeoutError("test did not release disable_all") self.disabled = True for motor in self.motors.values(): motor.enabled = False @@ -188,10 +236,11 @@ def test_connect_is_passive_and_reads_every_motor() -> None: for motor in controller.motors.values(): assert any(call[:2] == ("read", MECH_POS) for call in motor.calls) assert ("fault_report",) in motor.calls + assert ("active_report", True) in motor.calls assert ("enable",) not in motor.calls driver.close() - assert controller.disabled is False + assert controller.disabled is True assert controller.closed is True @@ -215,6 +264,59 @@ def test_enable_validates_then_holds_only_arm_motors() -> None: assert not controller.motors[7].enabled +def test_repeated_enable_preserves_an_enabled_gripper() -> None: + config = default_config() + calibrated = replace( + config, + gripper=replace(config.gripper, open_position=-1.0, closed_position=0.0), + ) + driver, controllers, _ = make_driver(config=calibrated) + driver.connect() + driver.enable() + driver.set_gripper(0.5) + + result = driver.enable() + + assert result["gripper_enabled"] is True + assert controllers[0].motors[7].enabled + assert driver.state()["gripper_enabled"] is True + + +def test_motion_refreshes_holds_for_every_enabled_motor() -> None: + config = default_config() + calibrated = replace( + config, + gripper=replace(config.gripper, open_position=-0.5, closed_position=0.5), + ) + driver, controllers, _ = make_driver(config=calibrated) + driver.connect() + driver.enable() + driver.set_gripper(0.5, duration_s=0.1) + controller = controllers[0] + + gripper_sends_before = sum( + call[0] == "send_mit" for call in controller.motors[7].calls + ) + driver.move_joints([0.0] * 6, duration_s=0.1) + gripper_sends_after = sum( + call[0] == "send_mit" for call in controller.motors[7].calls + ) + assert gripper_sends_after > gripper_sends_before + + arm_sends_before = { + motor_id: sum( + call[0] == "send_mit" for call in controller.motors[motor_id].calls + ) + for motor_id in range(1, 7) + } + driver.set_gripper(0.25, duration_s=0.1) + for motor_id in range(1, 7): + arm_sends_after = sum( + call[0] == "send_mit" for call in controller.motors[motor_id].calls + ) + assert arm_sends_after > arm_sends_before[motor_id] + + def test_enable_rejects_out_of_limit_startup_before_torque() -> None: driver, controllers, _ = make_driver({2: 0.2}) driver.connect() @@ -242,6 +344,18 @@ def test_enable_rejects_persistent_fault_before_torque() -> None: assert not controllers[0].enabled +def test_enable_rejects_missing_operation_status() -> None: + driver, controllers, _ = make_driver() + driver.connect() + controllers[0].motors[1].status_available = False + + with pytest.raises(RuntimeError, match="operation status"): + driver.enable() + + assert controllers[0].disabled + assert not controllers[0].enabled + + def test_enable_failure_disables_all_motors() -> None: driver, controllers, _ = make_driver() driver.connect() @@ -284,6 +398,70 @@ def enable() -> None: assert not controllers[0].enabled +def test_emergency_stop_invalidates_enable_that_already_passed_admission() -> None: + driver, controllers, _ = make_driver() + driver.connect() + original_sample = driver._sample_feedback + cancellation = BlockingSetEvent() + cancellation.block_next_set = True + driver.__dict__["_cancel_event"] = cancellation + sample_entered = threading.Event() + release_sample = threading.Event() + enable_error: list[BaseException] = [] + + def blocked_sample(operation_epoch: int): + sample_entered.set() + if not release_sample.wait(timeout=2.0): + raise TimeoutError("test did not release startup feedback") + return original_sample(operation_epoch) + + def enable() -> None: + try: + driver.enable() + except BaseException as exc: # captured for the test thread + enable_error.append(exc) + + driver._sample_feedback = blocked_sample + estop_thread = threading.Thread(target=driver.emergency_stop) + estop_thread.start() + assert cancellation.set_entered.wait(timeout=1.0) + enable_thread = threading.Thread(target=enable) + enable_thread.start() + + sample_entered.wait(timeout=0.2) + cancellation.release_set.set() + estop_thread.join(timeout=1.0) + release_sample.set() + enable_thread.join(timeout=1.0) + + assert not estop_thread.is_alive() + assert not enable_thread.is_alive() + assert enable_error and isinstance(enable_error[0], (MotionCancelled, RuntimeError)) + assert controllers[0].disabled + assert not controllers[0].enabled + assert driver.state()["stopped"] is True + + +def test_reset_stop_rejects_while_emergency_disable_is_in_progress() -> None: + driver, controllers, _ = make_driver() + driver.connect() + driver.enable() + controller = controllers[0] + controller.block_disable = True + + estop_thread = threading.Thread(target=driver.emergency_stop) + estop_thread.start() + assert controller.disable_entered.wait(timeout=1.0) + + try: + with pytest.raises(RuntimeError, match="stop.*in progress"): + driver.reset_stop() + finally: + controller.release_disable.set() + estop_thread.join(timeout=1.0) + assert not estop_thread.is_alive() + + def test_emergency_stop_wins_after_the_last_enable_hold() -> None: driver, controllers, _ = make_driver() driver.connect() @@ -377,6 +555,43 @@ def test_feedback_latency_does_not_create_catch_up_bursts() -> None: assert max(velocities) <= driver.config.joints[0].max_velocity * 1.02 +def test_emergency_stop_waits_for_at_most_one_feedback_read() -> None: + controllers: list[FakeController] = [] + + def factory(channel: str) -> FakeController: + controller = FakeController(channel) + controllers.append(controller) + return controller + + driver = RebotRobstrideDriver(default_config(), controller_factory=factory) + driver.connect() + driver.enable() + for motor in controllers[0].motors.values(): + motor.read_delay_s = 0.05 + motor.read_entered.clear() + + state_error: list[BaseException] = [] + + def read_state() -> None: + try: + driver.state() + except BaseException as exc: # captured for the test thread + state_error.append(exc) + + state_thread = threading.Thread(target=read_state) + state_thread.start() + assert controllers[0].motors[1].read_entered.wait(timeout=1.0) + + started = time.monotonic() + driver.emergency_stop() + elapsed = time.monotonic() - started + state_thread.join(timeout=1.0) + + assert elapsed < 0.25 + assert not state_thread.is_alive() + assert not state_error or isinstance(state_error[0], MotionCancelled) + + def test_motion_transport_failure_disables_all_and_latches_stop() -> None: driver, controllers, _ = make_driver() driver.connect() @@ -405,6 +620,41 @@ def test_motion_fault_report_disables_all() -> None: assert driver.state()["enabled"] is False +def test_arm_motion_fails_closed_on_enabled_gripper_fault() -> None: + config = default_config() + calibrated = replace( + config, + gripper=replace(config.gripper, open_position=-1.0, closed_position=0.0), + ) + driver, controllers, _ = make_driver(config=calibrated) + driver.connect() + driver.enable() + driver.set_gripper(0.5) + controllers[0].motors[7].fault_raw = 1 + + with pytest.raises(RuntimeError, match="fault"): + driver.move_joints([0.0] * 6, duration_s=0.1) + + assert controllers[0].disabled + + +def test_gripper_motion_fails_closed_on_enabled_arm_fault() -> None: + config = default_config() + calibrated = replace( + config, + gripper=replace(config.gripper, open_position=-1.0, closed_position=0.0), + ) + driver, controllers, _ = make_driver(config=calibrated) + driver.connect() + driver.enable() + controllers[0].motors[1].fault_raw = 1 + + with pytest.raises(RuntimeError, match="fault"): + driver.set_gripper(0.5) + + assert controllers[0].disabled + + def test_stalled_feedback_aborts_and_disables() -> None: driver, controllers, _ = make_driver() driver.connect() @@ -474,6 +724,39 @@ def test_move_joints_rejects_limit_and_duration_violations() -> None: driver.move_joints([0.0] * 6, duration_s=61.0) +def test_motion_server_deadline_precedes_client_timeout() -> None: + driver, controllers, clock = make_driver() + driver.connect() + driver.enable() + for motor in controllers[0].motors.values(): + motor.read_delay_s = 0.3 + + with pytest.raises(RuntimeError, match="server motion deadline"): + driver.move_joints([0.0] * 6, duration_s=60.0) + + assert clock() < 75.0 + assert controllers[0].disabled + + +def test_gripper_server_deadline_precedes_client_timeout() -> None: + config = default_config() + calibrated = replace( + config, + gripper=replace(config.gripper, open_position=-0.5, closed_position=0.5), + ) + driver, controllers, clock = make_driver(config=calibrated) + driver.connect() + driver.enable() + for motor in controllers[0].motors.values(): + motor.read_delay_s = 0.3 + + with pytest.raises(RuntimeError, match="server motion deadline"): + driver.set_gripper(0.5, duration_s=60.0) + + assert clock() < 75.0 + assert controllers[0].disabled + + def test_motion_requires_enable_and_respects_soft_stop() -> None: driver, _, _ = make_driver() driver.connect() @@ -608,11 +891,11 @@ def test_estop_between_gripper_ready_and_feedback_cannot_reenable() -> None: release_sample = threading.Event() motion_error: list[BaseException] = [] - def blocked_sample(): + def blocked_sample(operation_epoch: int, server_deadline: float | None = None): sample_entered.set() if not release_sample.wait(timeout=2.0): raise TimeoutError("test did not release feedback") - return original_sample() + return original_sample(operation_epoch, server_deadline) def move_gripper() -> None: try: @@ -656,6 +939,31 @@ def test_soft_stop_holds_enabled_gripper_at_measured_position() -> None: assert result["stopped"] is True +def test_soft_stop_persists_fresh_gripper_hold_after_reset() -> None: + config = default_config() + calibrated = replace( + config, + gripper=replace(config.gripper, open_position=-1.0, closed_position=0.0), + ) + driver, controllers, _ = make_driver(config=calibrated) + driver.connect() + driver.enable() + driver.set_gripper(0.5) + gripper = controllers[0].motors[7] + gripper.position = -0.4 + + driver.stop_motion() + driver.reset_stop() + command_count = sum(call[0] == "send_mit" for call in gripper.calls) + driver.move_joints([0.0] * 6, duration_s=0.1) + + later_commands = [call for call in gripper.calls if call[0] == "send_mit"][ + command_count: + ] + assert later_commands + assert all(call[1] == pytest.approx(-0.4) for call in later_commands) + + def test_gripper_transport_failure_disables_all() -> None: config = default_config() calibrated = replace( @@ -684,3 +992,37 @@ def test_close_always_disables_if_any_motor_was_enabled() -> None: assert controllers[0].disabled assert controllers[0].closed + + +def test_close_passive_connection_still_confirms_disable_and_is_idempotent() -> None: + driver, controllers, _ = make_driver() + driver.connect() + + driver.close() + driver.close() + + assert controllers[0].disabled + assert controllers[0].closed + + +def test_close_disable_failure_preserves_connected_uncertain_state() -> None: + driver, controllers, _ = make_driver() + driver.connect() + driver.enable() + controllers[0].fail_disable = True + + with pytest.raises(RuntimeError, match="injected disable failure"): + driver.close() + + state = driver.state() + assert state["connected"] is True + assert state["enabled"] is True + assert state["disable_failed"] is True + assert not controllers[0].closed + + controllers[0].fail_disable = False + driver.close() + driver.close() + + assert controllers[0].disabled + assert controllers[0].closed diff --git a/tests/robots/rebot_robstride/test_env_client.py b/tests/robots/rebot_robstride/test_env_client.py index 9655540a..a779c25a 100644 --- a/tests/robots/rebot_robstride/test_env_client.py +++ b/tests/robots/rebot_robstride/test_env_client.py @@ -1,5 +1,6 @@ from __future__ import annotations +from robots.rebot_robstride.config import MOTION_RPC_TIMEOUT_S from robots.rebot_robstride.env_client import RebotRobstrideEnvClient @@ -29,3 +30,8 @@ def test_client_uses_stable_robot_rpc_names() -> None: assert client.reset_stop()["method"] == "robot.reset_stop" assert client.emergency_stop()["method"] == "robot.emergency_stop" assert client.heartbeat()["method"] == "robot.heartbeat" + + timeouts = {method: timeout for method, _, _, timeout in rpc.calls} + assert timeouts["robot.enable"] == 30.0 + assert timeouts["robot.move_joints"] == MOTION_RPC_TIMEOUT_S + assert timeouts["robot.set_gripper"] == MOTION_RPC_TIMEOUT_S diff --git a/tests/robots/rebot_robstride/test_env_server.py b/tests/robots/rebot_robstride/test_env_server.py index fb54ad46..981b9a78 100644 --- a/tests/robots/rebot_robstride/test_env_server.py +++ b/tests/robots/rebot_robstride/test_env_server.py @@ -68,6 +68,19 @@ def test_shutdown_sets_event_after_emergency_stop() -> None: assert shutdown.is_set() +def test_shutdown_does_not_set_event_when_emergency_stop_fails() -> None: + class FailingDriver(FakeDriver): + def emergency_stop(self): + raise RuntimeError("disable failed") + + shutdown = threading.Event() + dispatch = make_dispatch(FailingDriver(), shutdown) + + with pytest.raises(RuntimeError, match="disable failed"): + dispatch("shutdown", (), {}) + assert not shutdown.is_set() + + @pytest.mark.parametrize("host", ["127.0.0.1", "127.0.0.2", "::1", "localhost"]) def test_loopback_hosts_are_accepted(host: str) -> None: validate_loopback_host(host) From 9bcaad87031e754dce7dc826e10b2d22f9c8543c Mon Sep 17 00:00:00 2001 From: johnnynunez Date: Mon, 20 Jul 2026 09:27:32 +0200 Subject: [PATCH 6/6] docs(rebot): record watchdog hardware validation --- docs/source-en/rst_source/usage/rebot_robstride.rst | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/docs/source-en/rst_source/usage/rebot_robstride.rst b/docs/source-en/rst_source/usage/rebot_robstride.rst index 78fa339e..abdbe8e7 100644 --- a/docs/source-en/rst_source/usage/rebot_robstride.rst +++ b/docs/source-en/rst_source/usage/rebot_robstride.rst @@ -168,9 +168,14 @@ shutdown mode. If disable fails, ``close`` does not close the controller or eras the physically uncertain enabled/``disable_failed`` state, allowing a later emergency-stop or close retry. The heartbeat protects loss of the agent process while the hardware server remains alive. A hard crash or ``SIGKILL`` of the -hardware server cannot be proven fail-safe because the tested -RobStride/motorbridge API does not expose a confirmed hardware watchdog. Never -operate unattended, and keep the physical emergency stop reachable. +hardware server cannot be proven fail-safe. Hardware validation on the B601 +firmware found RobStride ``0x7028 canTimeout`` present but disabled by default; +setting it to ``20000`` both at runtime and with saved parameters did not produce +a post-enable ``mode_state 2 -> 0`` transition during 21.5 seconds of host +silence in MIT mode. A manual disable was still required. Do not treat +``0x7028`` or motorbridge ``set_can_timeout_ms`` as a verified motor watchdog on +this platform. Never operate unattended, and keep the physical emergency stop +reachable. The first implementation provides guarded joint-space control. It does not claim collision avoidance, Cartesian planning, or perception-based grasping.