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..abdbe8e7 --- /dev/null +++ b/docs/source-en/rst_source/usage/rebot_robstride.rst @@ -0,0 +1,181 @@ +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 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 +------------- + +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. + +Safety-critical overrides have hard ceilings: control is 10–200 Hz, feedback +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 +--- + +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`` first validates raw joint +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, 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. +- ``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, 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 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. +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 +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. + +``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 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. 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. diff --git a/pyproject.toml b/pyproject.toml index d7d91fe2..14ddfae4 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" @@ -58,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 49f4e630..ebcaeda0 100644 --- a/robots/libero/__init__.py +++ b/robots/libero/__init__.py @@ -1,14 +1,21 @@ """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 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: @@ -26,6 +33,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..c701d839 --- /dev/null +++ b/robots/rebot_robstride/config.py @@ -0,0 +1,313 @@ +"""Validated configuration for the reBot DevArm RobStride backend.""" + +from __future__ import annotations + +import math +from dataclasses import dataclass, fields, replace +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: + """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 + feedback_rate_hz: float + read_timeout_ms: int + settle_tolerance: float + settle_timeout_s: float + 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 + + +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, + feedback_rate_hz=10.0, + read_timeout_ms=100, + settle_tolerance=0.03, + settle_timeout_s=2.0, + 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(), + ) + ) + + +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") + + 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", + "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} + + 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 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 <= 100 + ): + raise ValueError("read_timeout_ms must be an integer in [1, 100]") + + 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 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 not 1 <= config.settle_samples <= 10 + ): + 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") + + 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): + 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( + 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") + 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]") + + 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( + 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 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)) + 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") + 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/config/rebot_robstride.example.yaml b/robots/rebot_robstride/config/rebot_robstride.example.yaml new file mode 100644 index 00000000..38a945f0 --- /dev/null +++ b/robots/rebot_robstride/config/rebot_robstride.example.yaml @@ -0,0 +1,37 @@ +# 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 +feedback_rate_hz: 10 +read_timeout_ms: 100 +settle_tolerance: 0.03 +settle_timeout_s: 2.0 +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: + - {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..94fc0ef2 --- /dev/null +++ b/robots/rebot_robstride/driver.py @@ -0,0 +1,1055 @@ +"""Safety-focused motorbridge driver for the seven-motor reBot DevArm.""" + +from __future__ import annotations + +import math +import threading +import time +from collections.abc import Callable, Sequence +from typing import Any, NoReturn + +from robots.rebot_robstride.config import ( + SERVER_MOTION_DEADLINE_S, + GripperConfig, + JointConfig, + RebotConfig, +) + +MECH_POS = 0x7019 +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): + 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 + + # 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._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() + + @property + def connected(self) -> bool: + with self._state_lock: + return self._controller is not None + + @property + def enabled(self) -> bool: + with self._state_lock: + return self._enabled + + @property + def stopped(self) -> bool: + 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._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] + 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 + } + for motor in motors.values(): + motor.robstride_set_active_report(True) + 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 + except Exception: + try: + controller.close() + finally: + with self._state_lock: + self._controller = None + self._motors = {} + raise + + def state(self) -> dict[str, Any]: + """Return fresh positions, estimated velocities, and fault reports.""" + 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 + disable_failed = self._disable_failed + 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, + "disable_failed": disable_failed, + "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]: + """Validate startup state, enable arm joints, and hold observed pose.""" + with self._motion_lock: + 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" + ) + 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) + self._assert_operation_active(operation_epoch) + second = self._sample_feedback(operation_epoch) + self._validate_startup_positions(second) + self._validate_startup_velocity(second) + + arm_configs = list(self.config.joints) + try: + for motor_config in arm_configs: + with self._io_lock: + self._assert_operation_active(operation_epoch) + motor = self._motors[motor_config.motor_id] + motor.clear_error() + with self._io_lock: + self._assert_operation_active(operation_epoch) + 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._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._assert_operation_active(operation_epoch) + self._motors[motor_config.motor_id].enable() + for motor_config in arm_configs: + with self._io_lock: + self._assert_operation_active(operation_epoch) + self._send_mit_locked( + motor_config, + second["positions"][motor_config.motor_id], + ) + except Exception as exc: + self._fail_closed(exc) + + try: + with self._io_lock, self._state_lock: + self._assert_operation_active(operation_epoch) + self._enabled = True + 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": self._gripper_enabled, + "hold_positions": list(self._last_targets), + } + + def move_joints( + self, + positions: Sequence[float], + *, + duration_s: float = 2.0, + ) -> dict[str, Any]: + """Execute a monitored minimum-jerk trajectory and verify settlement.""" + with self._motion_lock: + 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") + if not all(math.isfinite(value) for value in target): + raise ValueError("joint targets must be finite") + self._validate_joint_limits(target) + requested_duration = self._validate_duration(duration_s) + + try: + 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 + ] + 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(requested_duration, minimum_duration) + self._check_actual_duration(actual_duration) + 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: + while True: + 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 + ) + scale = _min_jerk(progress) + waypoint = [ + initial + (goal - initial) * scale + for initial, goal in zip(start, target) + ] + 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(operation_epoch, server_deadline), + waypoint, + ) + next_feedback = self._clock() + feedback_interval + if progress >= 1.0: + break + + settled = self._wait_for_joint_target( + target, operation_epoch, server_deadline + ) + except MotionCancelled: + raise + 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": settled["positions"], + "final_velocities": settled["velocities"], + "max_error": max_error, + "reached": reached, + "enabled": self.enabled, + "requested_duration_s": requested_duration, + "actual_duration_s": actual_duration, + } + + def set_gripper( + self, + position: float, + *, + duration_s: float = 1.0, + ) -> dict[str, Any]: + """Enable and move the calibrated gripper; 0.0=open and 1.0=closed.""" + with self._motion_lock: + 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]") + 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" + ) + + try: + 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] + 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 + ) + 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, 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() + next_feedback = start_time + feedback_interval + + try: + while True: + if self._sleep_interruptible( + control_interval, operation_epoch, server_deadline + ): + raise MotionCancelled( + "gripper motion cancelled by stop request" + ) + progress = min( + max((self._clock() - start_time) / actual_duration, 0.0), 1.0 + ) + waypoint = start + (target - start) * _min_jerk(progress) + 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( + 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, operation_epoch, server_deadline + ) + 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": 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]: + """Preempt motion, latch a software stop, and hold fresh motor 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( + 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", + ) + 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 + with self._io_lock: + self._assert_epoch_current(stop_epoch) + self._send_mit_locked( + gripper, snapshot["positions"][gripper.motor_id] + ) + with self._state_lock: + if enabled: + self._last_targets = [ + snapshot["positions"][joint.motor_id] + for joint in self.config.joints + ] + 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" + ) + self._stopped = False + self._cancel_event.clear() + return {"enabled": self._enabled, "stopped": False} + + def emergency_stop(self) -> dict[str, Any]: + """Preempt motion and disable every motor without waiting for trajectory locks.""" + 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.""" + 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: + """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: + self._controller = None + self._motors = {} + self._enabled = False + self._gripper_enabled = False + self._stopped = True + 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, operation_epoch: int, server_deadline: float | None = None + ) -> dict[str, Any]: + self._require_connected() + 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 ( + 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( + self._motors[motor_id].robstride_get_param_f32( + MECH_POS, timeout_ms=self.config.read_timeout_ms + ) + ) + if not math.isfinite(position): + raise RuntimeError( + f"motor {motor_id} returned non-finite position feedback" + ) + return position + + 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 + ) -> None: + self._motors[config.motor_id].send_mit( + 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 + ] + 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: + 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) + 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"], + self._enabled_motor_ids(), + 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" + ) + + 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 | 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]["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, + 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._assert_operation_active(operation_epoch, server_deadline) + motor.clear_error() + with self._io_lock: + self._assert_operation_active(operation_epoch, server_deadline) + self._assert_no_faults( + self._read_faults_locked(), + self._enabled_motor_ids() | {gripper.motor_id}, + context="gripper startup", + ) + with self._io_lock: + self._assert_operation_active(operation_epoch, server_deadline) + motor.ensure_mode(MIT_MODE, timeout_ms=1000) + with self._io_lock: + self._assert_operation_active(operation_epoch, server_deadline) + motor.enable() + with self._io_lock: + self._assert_operation_active(operation_epoch, server_deadline) + self._send_mit_locked(gripper, hold) + with self._io_lock, self._state_lock: + 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], 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._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 + ] + 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 + ) + 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, + operation_epoch, + server_deadline, + ): + raise MotionCancelled("joint settlement cancelled by stop request") + + 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._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] + 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 { + "position": final_position, + "velocity": final_velocity, + "reached": False, + } + 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, + operation_epoch: int, + server_deadline: float | None = None, + ) -> bool: + deadline = self._clock() + max(0.0, duration_s) + while 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 _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) + 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: + 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._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: + 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): + 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: + 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) -> 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 new file mode 100644 index 00000000..98f59f84 --- /dev/null +++ b/robots/rebot_robstride/env_client.py @@ -0,0 +1,54 @@ +"""Agent-side RPC client for the reBot DevArm RobStride server.""" + +from __future__ import annotations + +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 +_ENABLE_TIMEOUT_S = 30.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=_ENABLE_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_RPC_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_RPC_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) + + 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 new file mode 100644 index 00000000..f18654e4 --- /dev/null +++ b/robots/rebot_robstride/env_server.py @@ -0,0 +1,178 @@ +#!/usr/bin/env python3 +"""Single-owner SocketCAN RPC server for the reBot DevArm RobStride arm.""" + +from __future__ import annotations + +import argparse +import ipaddress +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, + "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) + if handler is None: + raise ValueError(f"unknown RPC method: {method!r}") + return handler(*args, **kwargs) + + 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( + ["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() + validate_loopback_host(args.host) + 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), + 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() + + 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..c3e9ada1 --- /dev/null +++ b/robots/rebot_robstride/prompts/system.py @@ -0,0 +1,27 @@ +"""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 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. +""" + +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..e73ae94d --- /dev/null +++ b/robots/rebot_robstride/runtime.py @@ -0,0 +1,99 @@ +"""Process lifecycle adapter for the reBot DevArm RobStride environment.""" + +from __future__ import annotations + +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 +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 + 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: + 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)) + 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, + ) + self._process = None diff --git a/robots/rebot_robstride/toolkit.py b/robots/rebot_robstride/toolkit.py new file mode 100644 index 00000000..16a2d418 --- /dev/null +++ b/robots/rebot_robstride/toolkit.py @@ -0,0 +1,152 @@ +"""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, stop, and " + "disable-failure state. 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 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": []}, + }, + { + "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, + "maximum": 60.0, + "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, + "maximum": 60.0, + "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, + "maximum": 60.0, + "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, + "maximum": 60.0, + "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..61e2cee3 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, validate_env_args # 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,14 @@ 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 + 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 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 +303,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 +321,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, @@ -535,25 +345,26 @@ 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: - # 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, + "error": agent_error, "stats": stats, "messages": _serialize_messages(messages), } @@ -561,10 +372,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) @@ -579,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 5a03bb9e..a1eefff9 100644 --- a/rpent/envs/__init__.py +++ b/rpent/envs/__init__.py @@ -1,12 +1,16 @@ """Environment-specific RPent extensions.""" +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.base import get_env_spec, get_toolkit +from rpent.envs.runtime import EnvRuntime __all__ = [ "EnvSpec", + "EnvRuntime", "PromptBundle", "get_env_spec", + "get_runtime", "get_toolkit", + "validate_env_args", ] diff --git a/rpent/envs/base.py b/rpent/envs/base.py index cbe288e9..69f32660 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 @@ -16,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) @@ -36,9 +37,26 @@ 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) + + +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/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/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 new file mode 100644 index 00000000..24b79fe8 --- /dev/null +++ b/scripts/check_rebot_robstride.py @@ -0,0 +1,37 @@ +#!/usr/bin/env python3 +"""Passive seven-motor connectivity check for a reBot DevArm RobStride arm. + +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 + +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..801773c3 --- /dev/null +++ b/tests/envs/test_runtime_registry.py @@ -0,0 +1,62 @@ +from __future__ import annotations + +import sys +import types +from argparse import ArgumentParser, Namespace + +import pytest + +from rpent.envs import get_runtime, validate_env_args +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") + + +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 new file mode 100644 index 00000000..960a1042 --- /dev/null +++ b/tests/robots/rebot_robstride/test_config.py @@ -0,0 +1,118 @@ +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) + + +@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) + + +@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"), + ("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"), + ], +) +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 new file mode 100644 index 00000000..d3c2936b --- /dev/null +++ b/tests/robots/rebot_robstride/test_driver.py @@ -0,0 +1,1028 @@ +from __future__ import annotations + +import threading +import time +from dataclasses import replace +from types import SimpleNamespace + +import pytest + +from robots.rebot_robstride.config import default_config +from robots.rebot_robstride.driver import MotionCancelled, RebotRobstrideDriver + +MECH_POS = 0x7019 + + +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 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, + motor_id: int, + *, + position: float = 0.0, + clock=time.monotonic, + ) -> None: + self.motor_id = motor_id + self.position = position + 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() + self.block_send = False + 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) + 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 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: + 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.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: + self.position = pos + + +class FakeController: + 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.disabled = False + self.fail_disable = False + self.block_disable = False + self.disable_entered = threading.Event() + self.release_disable = threading.Event() + 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, + 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 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 + + def close(self) -> None: + self.closed = True + + +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, clock=fake_clock) + controllers.append(controller) + return controller + + driver = RebotRobstrideDriver( + config or default_config(), + controller_factory=factory, + clock=fake_clock, + sleep=fake_clock.sleep, + ) + 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) + + 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) + 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 True + assert controller.closed is True + + +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 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_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() + + 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_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() + controllers[0].motors[1].fail_on_send = True + + with pytest.raises(RuntimeError, match="injected send failure"): + driver.enable() + + assert controllers[0].disabled + assert driver.state()["enabled"] is False + + +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_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() + 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() + 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["final_velocities"] == pytest.approx([0.0] * 6) + 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 + + +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_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_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() + 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 False + assert state["stopped"] is True + 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_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() + 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_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_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() + 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_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() + + 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_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() + for motor in controllers[0].motors.values(): + motor.send_event.clear() + + motion_error: list[BaseException] = [] + + 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) + + 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() + + assert driver.enforce_heartbeat_deadman() is False + clock.sleep(driver.config.heartbeat_timeout_s + 0.01) + + 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, 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_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( + config, + gripper=replace(config.gripper, open_position=-1.0, closed_position=0.0), + ) + driver, controllers, _ = make_driver(config=calibrated) + driver.connect() + driver.enable() + + result = driver.set_gripper(0.25) + + assert result["target_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_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(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(operation_epoch, server_deadline) + + 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_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( + 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[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 + + +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 new file mode 100644 index 00000000..a779c25a --- /dev/null +++ b/tests/robots/rebot_robstride/test_env_client.py @@ -0,0 +1,37 @@ +from __future__ import annotations + +from robots.rebot_robstride.config import MOTION_RPC_TIMEOUT_S +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" + 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 new file mode 100644 index 00000000..981b9a78 --- /dev/null +++ b/tests/robots/rebot_robstride/test_env_server.py @@ -0,0 +1,92 @@ +from __future__ import annotations + +import threading + +import pytest + +from robots.rebot_robstride.env_server import make_dispatch, validate_loopback_host + + +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 heartbeat(self): + return {"op": "heartbeat"} + + +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"} + assert dispatch("robot.heartbeat", (), {}) == {"op": "heartbeat"} + + with pytest.raises(ValueError, match="unknown RPC method"): + dispatch("robot.set_zero", (), {}) + + +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() + + +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) + + +@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/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..4443f47d --- /dev/null +++ b/tests/robots/test_runtimes.py @@ -0,0 +1,146 @@ +from __future__ import annotations + +import threading +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 + + +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() + 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", FakeRebotEnv) + 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)] + 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"]