From ea53e7f4e0e10697e4d6e1524ca7d5d355a54947 Mon Sep 17 00:00:00 2001 From: yd-sl <166189879+yd-sl@users.noreply.github.com> Date: Tue, 29 Sep 2026 10:41:12 +0800 Subject: [PATCH] feat: add leader/follower gripper teleoperation Let one gripper mirror another: the leader goes slack so its jaws can be pushed by hand and publishes how far open it is, the follower drives its own jaws to match. The wire carries a normalized opening in [0, 1], not an angle, so the two ends need not share a calibration, mount, or zero point. The algorithm is ported from litearm_device.gripper_teleop, but the transport is a pluggable TeleopTransport instead of a hard zenoh dependency, so the SDK stays stdlib-only: UdpTeleopTransport for real links and InProcTeleopTransport for tests and two grippers in one process. The openness<->radian conversion carries close_sign, which the litearm original omitted, so a reverse-mounted follower moves the correct way. A follower that loses its leader holds its position at the follow gains rather than relaxing. --- README.md | 52 ++++ examples/teleop.py | 129 +++++++++ readme_zn.md | 47 ++++ src/litegrip/__init__.py | 29 ++ src/litegrip/gripper.py | 189 ++++++++++--- src/litegrip/teleop.py | 594 +++++++++++++++++++++++++++++++++++++++ tests/test_teleop.py | 296 +++++++++++++++++++ 7 files changed, 1302 insertions(+), 34 deletions(-) create mode 100644 examples/teleop.py create mode 100644 src/litegrip/teleop.py create mode 100644 tests/test_teleop.py diff --git a/README.md b/README.md index f827ac2..e0f5cc2 100644 --- a/README.md +++ b/README.md @@ -109,6 +109,58 @@ its own fails loudly (`False`, then `CommandError` from the motions) instead of silently adopting `can0`'s direction. Set `LITEGRIP_CALIB` to pin one explicit path for every channel instead. +## Leader/follower teleoperation + +Two grippers can be linked so one follows the other. The **master** (leader) motor goes slack — +you push its jaws by hand — and it publishes how far open it is at the loop rate. The **slave** +(follower) receives that and drives its own jaws to match. What travels over the wire is a +normalized opening in `[0, 1]`, not an angle, so the two ends do not need the same calibration, +mount, or zero point. + +```python +from litegrip import LiteGrip + +# Leader: publish this gripper's opening to the follower at 192.168.1.20. +with LiteGrip("can0") as master: + master.load_calibration() + master.enable() + master.teleop_start("master", host="192.168.1.20") + +# Follower: bind, align to the first frame, then follow. +with LiteGrip("can0") as slave: + slave.load_calibration() + slave.enable() + slave.teleop_start("slave", host="0.0.0.0") + while True: + print(slave.teleop_status()) # frames, openness, loop_hz, stale, ... +``` + +`examples/teleop.py` runs one end from the command line: + +```bash +# Machine A — the leader you push by hand: +python3 examples/teleop.py --mode master --channel can0 --host 192.168.1.20 +# Machine B — the follower: +python3 examples/teleop.py --mode slave --channel can0 --host 0.0.0.0 +``` + +Both ends must share `master_id` (default `master`) and be connected and enabled first. Teleop is +exclusive: the background loop owns the CAN I/O, so do not drive the gripper from the caller until +`teleop_stop()`. `teleop_start` returns the initial `teleop_status()` snapshot; `teleop_status()` +reports `active`, `mode`, `topic`, `frames`, `last_frame_age_ms`, `stale`, `openness`, `loop_hz`. + +- **The transport is plain UDP**, with no authentication or encryption. Use it only on a trusted + network. Pass `transport=` a `TeleopTransport` to supply your own; an injected one is never closed + by the SDK. +- **A follower that loses the leader holds its position, it does not go slack.** After + `watchdog_s` (default `0.2`) without a fresh frame it keeps commanding its last target under the + follow gains, so `stale` goes true but the jaws stay put — and can hold whatever is between them. +- **The follower clamps the incoming opening to `[0, 1]`**, i.e. to its own calibrated travel, so a + bad frame cannot command it past a limit. +- **Stopping leaves the gripper holding**, not slack: the master leaves zero-gravity mode on + `teleop_stop()`, so its jaws hold under the configured gains. +- Follow gains default to `kp=100.0`, `kd=2.0`; override with `kp=` / `kd=`. + ## The six actions These are the supported entry points for moving the gripper. Each one verifies its own diff --git a/examples/teleop.py b/examples/teleop.py new file mode 100644 index 0000000..4a32de4 --- /dev/null +++ b/examples/teleop.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3 +"""Run one end of a leader/follower gripper teleoperation link. + +This is a runnable companion to the teleoperation section of the README. It is +meant to be started once per machine — one process per gripper: + + # Machine A (the leader you push by hand): + python3 examples/teleop.py --mode master --channel can0 --host 192.168.1.20 + + # Machine B (the follower that copies it): + python3 examples/teleop.py --mode slave --channel can0 --host 0.0.0.0 + +Both ends must share ``--master-id``. The default transport is plain UDP on +``--port``; it carries no authentication or encryption, so keep it on a trusted +network. Press Ctrl+C on either end to stop; the gripper holds its position. + +This script talks to real hardware. It does not detect an object in the jaws, +and the follower holds its position on a leader dropout rather than going +slack, so it can clamp whatever is between the fingers. Keep a hand on the +power switch. +""" + +from __future__ import annotations + +import argparse +import sys +import time + +from litegrip import LiteGrip, LiteGripError + + +def build_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser( + description="Run one end of a LiteGrip leader/follower teleop link.") + parser.add_argument( + "--mode", required=True, choices=("master", "slave"), + help="master = the leader you push by hand; slave = the follower") + parser.add_argument( + "--channel", default="can0", help="CAN interface (default: can0)") + parser.add_argument( + "--can-id", type=lambda s: int(s, 0), default=0x08, + help="motor CAN ID (default: 0x08)") + parser.add_argument( + "--host", required=True, + help="master: the follower's address; slave: the local bind address") + parser.add_argument( + "--port", type=int, default=7448, help="UDP port (default: 7448)") + parser.add_argument( + "--master-id", default="master", + help="topic id both ends must agree on (default: master)") + parser.add_argument( + "--mount", choices=("normal", "reverse"), default=None, + help="load a mount template instead of this channel's calibration") + parser.add_argument( + "--kp", type=float, default=None, + help="follower stiffness (default: 100.0)") + parser.add_argument( + "--kd", type=float, default=None, + help="follower damping (default: 2.0)") + parser.add_argument( + "--no-align", action="store_true", + help="follower: skip the one-shot align to the first frame") + parser.add_argument( + "--watchdog", type=float, default=0.2, + help="follower: hold position after this many seconds without a " + "fresh frame (default: 0.2)") + parser.add_argument( + "--rate", type=float, default=50.0, help="loop rate in Hz (default: 50)") + parser.add_argument( + "--dry-run", action="store_true", + help="print the resolved plan and exit without touching hardware") + return parser + + +def _print_status(status: dict) -> None: + age = status.get("last_frame_age_ms") + age_txt = "-" if age is None else f"{age:6.1f}" + openness = status.get("openness") + open_txt = "-" if openness is None else f"{openness:5.3f}" + print(f"frames={status.get('frames', 0):>7} " + f"age_ms={age_txt} stale={str(status.get('stale', False)):>5} " + f"openness={open_txt} loop_hz={status.get('loop_hz', 0.0):4.1f}", + flush=True) + + +def main(argv: list[str] | None = None) -> int: + args = build_parser().parse_args(argv) + + gripper = LiteGrip(channel=args.channel, can_id=args.can_id) + if args.mount is not None: + gripper.load_calibration(template=args.mount) + else: + gripper.load_calibration() + print(f"mount={gripper.mount} closed={gripper.config.pos_closed_rad:+.4f} " + f"open={gripper.config.pos_open_rad:+.4f} rad_to_mm={gripper.config.rad_to_mm}") + + if args.dry_run: + print(f"dry run: would start {args.mode} on {args.channel} at " + f"{args.host}:{args.port} (topic litegrip/teleop/{args.master_id})") + return 0 + + gripper.connect() + gripper.enable() + status = gripper.teleop_start( + args.mode, host=args.host, port=args.port, + kp=args.kp, kd=args.kd, align=not args.no_align, + watchdog_s=args.watchdog, rate_hz=args.rate, + master_id=args.master_id) + print(f"teleop {args.mode} running; Ctrl+C to stop") + _print_status(status) + + try: + while True: + time.sleep(1.0) + _print_status(gripper.teleop_status()) + except KeyboardInterrupt: + print("\nstopping") + finally: + gripper.teleop_stop() + gripper.disconnect() + return 0 + + +if __name__ == "__main__": + try: + sys.exit(main()) + except LiteGripError as error: + print(f"error: {error}", file=sys.stderr) + sys.exit(1) diff --git a/readme_zn.md b/readme_zn.md index 53645f1..57b6f70 100644 --- a/readme_zn.md +++ b/readme_zn.md @@ -98,6 +98,53 @@ with LiteGrip("can1", mount="reverse") as gripper: `False`，随后运动接口抛 `CommandError`），而不是悄悄采纳 `can0` 的方向。想让所有通道共用一个 显式路径，设 `LITEGRIP_CALIB`。 +## 主从遥操 + +两台夹爪可以联动，一台跟着另一台动。**主夹爪**（leader）的电机卸力 —— 你用手掰它的爪子， +它按循环频率把「张开程度」发出去；**从夹爪**（follower）收到后驱动自己的爪子跟到位。线上传的 +是归一化到 `[0, 1]` 的张开度，不是角度，所以两端不需要相同的标定、装法或零点。 + +```python +from litegrip import LiteGrip + +# 主端：把本夹爪的张开度发到 192.168.1.20 的从端。 +with LiteGrip("can0") as master: + master.load_calibration() + master.enable() + master.teleop_start("master", host="192.168.1.20") + +# 从端：绑定端口，先对齐首帧，然后跟随。 +with LiteGrip("can0") as slave: + slave.load_calibration() + slave.enable() + slave.teleop_start("slave", host="0.0.0.0") + while True: + print(slave.teleop_status()) # frames, openness, loop_hz, stale, ... +``` + +`examples/teleop.py` 可以在命令行跑其中一端： + +```bash +# A 机 —— 你用手掰的主夹爪： +python3 examples/teleop.py --mode master --channel can0 --host 192.168.1.20 +# B 机 —— 从夹爪： +python3 examples/teleop.py --mode slave --channel can0 --host 0.0.0.0 +``` + +两端必须共用 `master_id`（默认 `master`），且都已连接、已使能。遥操是互斥的：后台循环独占 CAN +读写，在 `teleop_stop()` 之前不要再从调用方驱动夹爪。`teleop_start` 返回初始的 +`teleop_status()`；`teleop_status()` 报告 `active`、`mode`、`topic`、`frames`、 +`last_frame_age_ms`、`stale`、`openness`、`loop_hz`。 + +- **传输是明文 UDP**，无鉴权、无加密，只用在可信网络里。要给自定义传输，传 `transport=` 一个 + `TeleopTransport`；注入的传输不会被 SDK 关闭。 +- **从端与主端失联时是「持位」，不是「卸力」。** 超过 `watchdog_s`（默认 `0.2`）没有新帧后， + 它仍按跟随增益顶着上一个目标继续发帧 —— 于是 `stale` 变真，但爪子停在原地，可能夹住中间的 + 东西。 +- **从端会把收到的张开度夹到 `[0, 1]`**，也就是夹在自己的标定行程内，坏帧无法把它指到限位之外。 +- **停止后是持位**，不是卸力：主端在 `teleop_stop()` 时退出零重力模式，爪子按配置增益持位。 +- 跟随增益默认 `kp=100.0`、`kd=2.0`，用 `kp=` / `kd=` 覆盖。 + ## 六个动作接口 要让夹爪动起来就用这六个。每一个都会自己校验结果再报成功，所以调用方不必再重写斜坡和 diff --git a/src/litegrip/__init__.py b/src/litegrip/__init__.py index 21db9d2..232a1a4 100644 --- a/src/litegrip/__init__.py +++ b/src/litegrip/__init__.py @@ -107,6 +107,22 @@ def _detect_version(dist_name: str = "litegrip") -> str: NotInitializedError, ) +# ── Teleoperation (leader / follower) ─────────────────────────────────── +from .teleop import ( + GripperTeleop, + TeleopTransport, + TeleopSubscription, + UdpTeleopTransport, + InProcTeleopTransport, + TeleopError, + TeleopBusyError, + TeleopNotActiveError, + FRAME_SIZE, + encode_frame, + decode_frame, + teleop_topic, +) + # ── CAN subpackage (expert) ───────────────────────────────────────────── from . import can @@ -150,6 +166,19 @@ def _detect_version(dist_name: str = "litegrip") -> str: "CANTimeoutError", "HardwareError", "NotInitializedError", + # Teleoperation + "GripperTeleop", + "TeleopTransport", + "TeleopSubscription", + "UdpTeleopTransport", + "InProcTeleopTransport", + "TeleopError", + "TeleopBusyError", + "TeleopNotActiveError", + "FRAME_SIZE", + "encode_frame", + "decode_frame", + "teleop_topic", # Subpackages "can", ] diff --git a/src/litegrip/gripper.py b/src/litegrip/gripper.py index fbeb5d3..f687067 100644 --- a/src/litegrip/gripper.py +++ b/src/litegrip/gripper.py @@ -27,6 +27,7 @@ import os as _os import select as _select_mod import sys +import threading import time from datetime import datetime from typing import Callable, List, Optional @@ -220,6 +221,15 @@ def __init__( self._status_flags = GripperStatus.NONE self._actions = GripperActions(self, motion_config) + # Reentrant lock around the low-level CAN I/O that a teleop background + # thread shares with the caller (send/poll/get_state). A no-op for the + # single-threaded use this SDK assumed before teleop existed. + self._io_lock = threading.RLock() + self._teleop: Optional["GripperTeleop"] = None + # Transport teleop_start built itself (as opposed to one the caller + # injected), so teleop_stop knows what it is allowed to close. + self._teleop_transport: Optional["TeleopTransport"] = None + # Declaring the mount is just loading the matching template, so it # costs no CAN traffic and is safe this early. The template carries # only a direction and geometry, so it cannot clobber the identity @@ -335,6 +345,9 @@ def disconnect(self) -> None: if not self._connected: return + if self._teleop is not None: + self.teleop_stop() + if self._can: self._can.disconnect(disable=self._disable_on_disconnect) self._can = None @@ -467,8 +480,9 @@ def send_mit_frame( """ if self._can is None or not self._enabled: return False - return self._can.control_mit( - q_target=q, kp=kp, kd=kd, dq_target=dq, tau_feedforward=tau) + with self._io_lock: + return self._can.control_mit( + q_target=q, kp=kp, kd=kd, dq_target=dq, tau_feedforward=tau) def poll(self, timeout_s: float = 0.0) -> bool: """Poll for one CAN frame and update cached motor state. @@ -481,7 +495,8 @@ def poll(self, timeout_s: float = 0.0) -> bool: """ if self._can is None: return False - return self._can.poll(timeout_s=timeout_s) + with self._io_lock: + return self._can.poll(timeout_s=timeout_s) # ═══════════════════════════════════════════════════════════════════ # Zero-gravity mode (manual back-driving) @@ -1542,38 +1557,39 @@ def get_state(self, wait: bool = True) -> GripperState: if self._can is None: return GripperState() - if wait: - self._can.update_state(timeout_s=0.05) - else: - self._can.poll(timeout_s=0.0) - - position_rad = self._can.get_position() - velocity_rad_s = self._can.get_velocity() - torque_nm = self._can.get_torque() - error_code = self._can.get_error() - t_mos, t_coil = self._can.get_temperature() + with self._io_lock: + if wait: + self._can.update_state(timeout_s=0.05) + else: + self._can.poll(timeout_s=0.0) - # pos_closed_rad = closed (0 mm), pos_open_rad = open (max mm). Which - # way the rad count runs depends on the mount, so close_sign sets the - # sign; the result is 0 mm at the closed limit and +stroke at the open - # limit for both mountings. - s = self._config.close_sign - position_mm = ((self._config.pos_closed_rad - position_rad) - * s * self._config.rad_to_mm) - # Squeeze is positive force: the sign flips with the mount too. - force_n = s * torque_nm * UnitConversion.NM_TO_N - - return GripperState( - position_rad=position_rad, - velocity_rad_s=velocity_rad_s, - torque_nm=torque_nm, - temperature_mos=t_mos, - temperature_coil=t_coil, - error_code=error_code, - timestamp=time.time(), - position_mm=position_mm, - force_n=force_n, - ) + position_rad = self._can.get_position() + velocity_rad_s = self._can.get_velocity() + torque_nm = self._can.get_torque() + error_code = self._can.get_error() + t_mos, t_coil = self._can.get_temperature() + + # pos_closed_rad = closed (0 mm), pos_open_rad = open (max mm). + # Which way the rad count runs depends on the mount, so close_sign + # sets the sign; the result is 0 mm at the closed limit and +stroke + # at the open limit for both mountings. + s = self._config.close_sign + position_mm = ((self._config.pos_closed_rad - position_rad) + * s * self._config.rad_to_mm) + # Squeeze is positive force: the sign flips with the mount too. + force_n = s * torque_nm * UnitConversion.NM_TO_N + + return GripperState( + position_rad=position_rad, + velocity_rad_s=velocity_rad_s, + torque_nm=torque_nm, + temperature_mos=t_mos, + temperature_coil=t_coil, + error_code=error_code, + timestamp=time.time(), + position_mm=position_mm, + force_n=force_n, + ) def get_position(self) -> float: """Current position in mm.""" @@ -1664,6 +1680,111 @@ def read_param(self, rid: int, timeout_s: float = 0.5) -> float: raise NotInitializedError("未连接") return self._can.read_param(rid, timeout_s=timeout_s) + # ═══════════════════════════════════════════════════════════════════ + # Teleoperation (leader / follower) + # ═══════════════════════════════════════════════════════════════════ + + def teleop_start( + self, + mode: str, + transport: Optional["TeleopTransport"] = None, + host: Optional[str] = None, + port: int = 7448, + kp: Optional[float] = None, + kd: Optional[float] = None, + align: bool = True, + watchdog_s: float = 0.2, + rate_hz: float = 50.0, + master_id: str = "master", + ) -> dict: + """Start leader/follower teleoperation on this gripper. + + ``mode="master"`` (leader) makes the motor slack — the jaws can be + pushed by hand — and publishes the opening. ``mode="slave"`` + (follower) receives the opening and follows it. + + Both ends must agree on ``master_id``. Teleoperation is exclusive: + the background loop owns the CAN I/O until :meth:`teleop_stop`, so do + not drive this gripper from the caller while it runs. + + Args: + mode: ``"master"`` or ``"slave"``. + transport: A :class:`~litegrip.TeleopTransport`. When omitted, a + :class:`~litegrip.UdpTeleopTransport` is built — the master + sends to ``host:port`` (the follower's address), the slave + binds ``host:port``. An injected transport is never closed by + this class. + host: Address for the default UDP transport (required when + ``transport`` is omitted). + port: UDP port for the default transport. + kp, kd: Follow gains (slave). ``None`` uses 100.0 / 2.0. + align: Slave only — align to the first received frame before + following. + watchdog_s: Slave only — hold position after this long without a + fresh frame. + rate_hz: Loop rate. + master_id: Topic id shared by both ends. + + Returns: + The initial :meth:`teleop_status` snapshot. + + Raises: + TeleopBusyError: teleoperation is already running. + NotInitializedError: not connected or not enabled. + """ + from .teleop import (GripperTeleop, TeleopBusyError, + UdpTeleopTransport, teleop_topic) + + self._check_connected() + self._check_enabled() + if mode not in ("master", "slave"): + raise ValueError(f"mode must be 'master' or 'slave', got {mode!r}") + if self._teleop is not None and self._teleop.is_running: + raise TeleopBusyError("teleop is already running") + + created_transport = None + if transport is None: + if host is None: + raise ValueError("host is required when no transport is given") + addr = f"{host}:{port}" + if mode == "master": + transport = created_transport = UdpTeleopTransport(pub_addr=addr) + else: + transport = created_transport = UdpTeleopTransport(bind_addr=addr) + + manager = GripperTeleop( + self, transport, mode, teleop_topic(master_id), + rate_hz=rate_hz, kp=kp, kd=kd, align=align, watchdog_s=watchdog_s) + manager.start() + self._teleop = manager + self._teleop_transport = created_transport + return manager.status() + + def teleop_stop(self, timeout: float = 2.0) -> dict: + """Stop teleoperation and leave the gripper holding its position. + + The master also leaves zero-gravity mode, so the jaws hold under the + configured gains rather than falling slack. + """ + manager = self._teleop + if manager is None: + return {"active": False, "mode": None} + manager.stop(timeout=timeout) + self._teleop = None + if self._teleop_transport is not None: + try: + self._teleop_transport.close() + except Exception as e: # noqa: BLE001 + log.debug("teleop transport close failed: %s", e) + self._teleop_transport = None + return manager.status() + + def teleop_status(self) -> dict: + """Snapshot of the running teleoperation, or ``{"active": False}``.""" + if self._teleop is None: + return {"active": False, "mode": None} + return self._teleop.status() + # ═══════════════════════════════════════════════════════════════════ # Internal # ═══════════════════════════════════════════════════════════════════ diff --git a/src/litegrip/teleop.py b/src/litegrip/teleop.py new file mode 100644 index 0000000..0c7cdce --- /dev/null +++ b/src/litegrip/teleop.py @@ -0,0 +1,594 @@ +"""Leader/follower gripper teleoperation — single-DOF position mirroring. + +Ported from ``litearm_device.gripper_teleop`` (the implementation that drove the +same feature inside the litearm server stack), minus the parts that only made +sense there. The algorithm is unchanged; what is new is that the transport is +pluggable and the module depends on nothing outside the standard library, so a +bare ``LiteGrip`` on a CAN bus is all it takes. + +Topology:: + + leader (zero-gravity, hand-back-driven) --pub--> follower (MIT follow) + +``master`` streams zero-torque frames so the jaws can be pushed by hand, and +publishes its normalised opening at ``rate_hz``. ``slave`` subscribes, aligns +once, then streams MIT position frames toward the received opening. + +Why the wire carries ``openness`` and not radians +------------------------------------------------- +Each gripper has its own zero, direction and calibration (one unit opens at +-1.42 rad, another at +1.14 rad), so a raw angle is meaningless on the far +side. ``openness`` is the opening normalised by the *local* travel and is +therefore dimensionless and direction-free; each side converts on its own. + +Frame layout (big-endian, four doubles, 32 bytes) — byte-compatible with the +litearm implementation so the two can interoperate:: + + openness[0..1] | position_mm | force_n | timestamp + +Safety notes +------------ +* ``openness`` is clamped to ``[0, 1]``, which keeps every commanded target + inside the calibrated travel. That clamp is the only limit this layer + applies; there is no red-line logic here. +* A ``slave`` whose leader goes quiet **holds** its last target at the follow + gains (it does not relax to zero torque). The jaws therefore keep pressing + whatever is between them — the same behaviour as the litearm original. +* Teleoperation is exclusive: stop any motion you started elsewhere before + calling :meth:`~litegrip.LiteGrip.teleop_start`. +* :class:`UdpTeleopTransport` is plain, unauthenticated UDP. Use it only on a + trusted network. +""" + +from __future__ import annotations + +import logging +import socket +import struct +import threading +import time +from collections import deque +from typing import Any, Callable, Dict, Optional, Tuple, Union + +from .exceptions import LiteGripError + +log = logging.getLogger("litegrip.teleop") + +# ── Frame codec ─────────────────────────────────────────────────────────── + +_FRAME = struct.Struct(">4d") + +#: Size of one teleop frame in bytes (four doubles). +FRAME_SIZE = _FRAME.size + + +def encode_frame(openness: float, position_mm: float, force_n: float, + timestamp: float) -> bytes: + """Pack one teleop frame. + + Args: + openness: Normalised opening in ``[0, 1]`` (0 = closed, 1 = fully + open). The quantity that actually drives the follower. + position_mm: Leader opening in mm — diagnostic only. + force_n: Leader gripping force in N — diagnostic only. + timestamp: Sender clock in seconds — diagnostic only; the follower + judges liveness from its own receive time, not from this field. + """ + return _FRAME.pack(float(openness), float(position_mm), float(force_n), + float(timestamp)) + + +def decode_frame(payload: bytes) -> Tuple[float, float, float, float]: + """Unpack a teleop frame into ``(openness, position_mm, force_n, timestamp)``. + + Raises: + ValueError: ``payload`` is not exactly :data:`FRAME_SIZE` bytes. + """ + if len(payload) != FRAME_SIZE: + raise ValueError( + f"teleop frame must be {FRAME_SIZE} bytes, got {len(payload)}") + return _FRAME.unpack(payload) + + +def teleop_topic(master_id: str = "master") -> str: + """Topic the leader publishes and the follower subscribes to. + + Both ends must agree on ``master_id``; it defaults to ``"master"`` so a + single pair needs no configuration. Use a distinct id per pair when more + than one teleoperation runs on the same transport. + """ + return f"litegrip/teleop/{master_id}" + + +# ── Transport abstraction ───────────────────────────────────────────────── + + +class TeleopSubscription: + """A non-blocking subscription handle.""" + + def try_recv(self) -> Optional[bytes]: + """Return the next payload, or ``None`` if none is waiting.""" + raise NotImplementedError + + def drain_latest(self) -> Optional[bytes]: + """Discard all but the newest queued payload and return it. + + Teleoperation only ever wants the latest sample, so a slow consumer + skips history instead of replaying it. + """ + latest = None + while True: + msg = self.try_recv() + if msg is None: + return latest + latest = msg + + +class TeleopTransport: + """Publish/subscribe transport between a leader and a follower. + + Implement this to carry teleop frames over anything (a different network + stack, an in-process bus, ...). Two implementations ship with the SDK: + :class:`UdpTeleopTransport` and :class:`InProcTeleopTransport`. + """ + + def pub(self, topic: str, payload: bytes) -> None: + raise NotImplementedError + + def sub(self, topic: str) -> TeleopSubscription: + raise NotImplementedError + + def close(self) -> None: + """Release any resources. Idempotent.""" + + +def _parse_addr(addr: Union[str, Tuple[str, int]]) -> Tuple[str, int]: + if isinstance(addr, str): + host, _, port = addr.rpartition(":") + if not host or not port: + raise ValueError(f"address must be 'host:port', got {addr!r}") + return host, int(port) + host, port = addr + return str(host), int(port) + + +class _UdpSubscription(TeleopSubscription): + def __init__(self, sock: socket.socket) -> None: + self._sock = sock + + def try_recv(self) -> Optional[bytes]: + try: + data, _ = self._sock.recvfrom(2048) + return data + except (BlockingIOError, InterruptedError): + return None + except OSError as e: + log.debug("udp recv failed: %s", e) + return None + + +class UdpTeleopTransport(TeleopTransport): + """Plain UDP transport — the zero-dependency default. + + The leader sends frames to ``pub_addr``; the follower receives on + ``bind_addr``. Either or both may be given, so one object can both send + and receive (not needed for a single leader/follower pair). Same machine: + ``"127.0.0.1:7448"``. Across machines: the peer's real address, + ``"0.0.0.0:"`` to accept on every interface. + + Unauthenticated and unencrypted — trusted networks only. A dropped + datagram is simply the next sample being late, which the follower's + watchdog already tolerates. + """ + + def __init__(self, pub_addr: Union[str, Tuple[str, int], None] = None, + bind_addr: Union[str, Tuple[str, int], None] = None) -> None: + self._pub_addr = _parse_addr(pub_addr) if pub_addr is not None else None + self._sock: Optional[socket.socket] = None + self._sub: Optional[_UdpSubscription] = None + + if bind_addr is not None: + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) + sock.bind(_parse_addr(bind_addr)) + sock.setblocking(False) + self._sock = sock + elif self._pub_addr is not None: + self._sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + + if self._pub_addr is None and bind_addr is None: + raise ValueError("UdpTeleopTransport needs pub_addr, bind_addr or both") + + def pub(self, topic: str, payload: bytes) -> None: + if self._pub_addr is None or self._sock is None: + return + try: + self._sock.sendto(payload, self._pub_addr) + except OSError as e: + log.debug("udp send failed: %s", e) + + def sub(self, topic: str) -> TeleopSubscription: + if self._sock is None: + raise TeleopError( + "UdpTeleopTransport was built without bind_addr; it cannot subscribe") + if self._sub is None: + self._sub = _UdpSubscription(self._sock) + return self._sub + + def close(self) -> None: + if self._sock is not None: + try: + self._sock.close() + except OSError: + pass + self._sock = None + self._sub = None + + +class _InProcSubscription(TeleopSubscription): + def __init__(self, queue: "deque[bytes]") -> None: + self._queue = queue + + def try_recv(self) -> Optional[bytes]: + try: + return self._queue.popleft() + except IndexError: + return None + + +class InProcTeleopTransport(TeleopTransport): + """In-process bus — for tests and for two grippers in one program. + + A single instance is shared by both ends; nothing crosses a process + boundary. Each topic keeps a bounded FIFO per subscriber, so a subscriber + that falls behind skips frames rather than growing without bound. + """ + + def __init__(self, fifo_depth: int = 16) -> None: + self._depth = fifo_depth + self._queues: Dict[str, list] = {} + self._lock = threading.Lock() + + def pub(self, topic: str, payload: bytes) -> None: + with self._lock: + for queue in self._queues.get(topic, []): + queue.append(payload) + while len(queue) > self._depth: + queue.popleft() + + def sub(self, topic: str) -> TeleopSubscription: + queue: "deque[bytes]" = deque() + with self._lock: + self._queues.setdefault(topic, []).append(queue) + return _InProcSubscription(queue) + + def close(self) -> None: + with self._lock: + self._queues.clear() + + +# ── openness <-> radians conversion ─────────────────────────────────────── + + +def travel_mm(cfg: Any) -> float: + """Full calibrated stroke in mm (``|open - closed| * rad_to_mm``).""" + return abs(cfg.pos_open_rad - cfg.pos_closed_rad) * cfg.rad_to_mm + + +def rad_to_openness(position_rad: float, cfg: Any) -> float: + """Motor angle -> normalised opening in ``[0, 1]``. + + Uses the same sign convention as :meth:`LiteGrip.get_state`, so it is + correct for both mountings (``cfg.close_sign`` carries the direction). + """ + stroke = travel_mm(cfg) + if stroke <= 0.0: + return 0.0 + position_mm = ((cfg.pos_closed_rad - position_rad) + * cfg.close_sign * cfg.rad_to_mm) + return _clamp01(position_mm / stroke) + + +def openness_to_rad(openness: float, cfg: Any) -> float: + """Normalised opening in ``[0, 1]`` -> motor angle. + + Mirrors :meth:`LiteGrip.goto_mm` including its ``close_sign`` factor, so a + reverse-mounted follower moves the correct way. ``openness`` is clamped + to ``[0, 1]`` first, which bounds the target to the calibrated travel. + """ + if cfg.rad_to_mm <= 0.0: + return cfg.pos_closed_rad + openness = _clamp01(openness) + return (cfg.pos_closed_rad + - cfg.close_sign * openness * travel_mm(cfg) / cfg.rad_to_mm) + + +def _clamp01(x: float) -> float: + return 0.0 if x < 0.0 else (1.0 if x > 1.0 else x) + + +# ── exceptions ──────────────────────────────────────────────────────────── + + +class TeleopError(LiteGripError): + """Base class for teleoperation errors.""" + + +class TeleopBusyError(TeleopError): + """Raised when teleop is started while it is already running.""" + + +class TeleopNotActiveError(TeleopError): + """Raised when an operation needs an active session but none is running.""" + + +# ── the algorithm ───────────────────────────────────────────────────────── + + +class GripperTeleop: + """One side of a gripper teleoperation, driven by a background thread. + + ``mode="master"`` streams zero-torque frames (so the jaws can be moved by + hand) and publishes the opening. ``mode="slave"`` subscribes, aligns to + the first frame with a single :meth:`~litegrip.LiteGrip.goto_rad`, then + follows every fresh sample with ``send_mit_frame``. + + One loop thread per side, sampling and sending in the same cycle — no + shared buffers and no contention, which is all a single-DOF gripper needs + (see the litearm original for the reasoning). + + Args: + gripper: The ``LiteGrip`` this side drives. + transport: Transport used to publish (master) and, unless + ``sub_transport`` is given, to subscribe (slave). + mode: ``"master"`` or ``"slave"``. + topic: Topic to publish/subscribe. + rate_hz: Loop rate. ~50 Hz is plenty; the CAN frame stream and the + publish share the same cycle. + kp, kd: Follow gains (slave). ``None`` uses ``100.0`` / ``2.0``. + align: Slave only — align to the first frame before following. + watchdog_s: Slave only — seconds without a fresh frame before the + follower is considered stale and starts holding. + sub_transport: Slave only — a separate transport to subscribe on when + the leader is remote (the master's transport is local-only). + sleep_fn, time_fn: Timing seams for tests. ``time_fn`` must be + monotonic. + """ + + def __init__( + self, + gripper: Any, + transport: TeleopTransport, + mode: str, + topic: str, + rate_hz: float = 50.0, + kp: Optional[float] = None, + kd: Optional[float] = None, + align: bool = True, + watchdog_s: float = 0.2, + sub_transport: Optional[TeleopTransport] = None, + sleep_fn: Callable[[float], None] = time.sleep, + time_fn: Callable[[], float] = time.monotonic, + ) -> None: + if mode not in ("master", "slave"): + raise ValueError(f"mode must be 'master' or 'slave', got {mode!r}") + if rate_hz <= 0.0: + raise ValueError("rate_hz must be > 0") + self._g = gripper + self._tp = transport + self._mode = mode + self._topic = topic + self._dt = 1.0 / rate_hz + self._kp = kp + self._kd = kd + self._align = align + self._watchdog_s = watchdog_s + self._sub_tp = sub_transport + self._sleep_fn = sleep_fn + self._time_fn = time_fn + + self._thread: Optional[threading.Thread] = None + self._running = False + + # Diagnostics. + self._frames = 0 + self._last_openness = 0.0 + self._last_frame_ts = 0.0 + self._stale = False + self._loops = 0 + self._loop_hz = 0.0 + self._hz_t0 = 0.0 + self._hz_n0 = 0 + + # ── lifecycle ───────────────────────────────────────────────────── + + @property + def is_running(self) -> bool: + return self._running + + def start(self) -> None: + """Start the background loop. Raises :class:`TeleopBusyError` if it + is already running.""" + if self._running: + raise TeleopBusyError("teleop already running") + self._running = True + self._thread = threading.Thread( + target=self._run, name=f"litegrip-teleop-{self._mode}", daemon=True) + self._thread.start() + log.info("teleop started: mode=%s topic=%s rate=%.0fHz", + self._mode, self._topic, 1.0 / self._dt) + + def _run(self) -> None: + # Whatever ends the loop — a stop, a CAN error, a bad transport — the + # session is no longer active once the thread returns. + try: + if self._mode == "master": + self._master_loop() + else: + self._slave_loop() + finally: + self._running = False + + def stop(self, timeout: float = 2.0) -> None: + """Stop the loop and leave the gripper holding its position. + + The master also leaves zero-gravity mode, so the jaws hold under the + configured gains instead of falling slack. + """ + self._running = False + thread = self._thread + if thread is not None and thread.is_alive(): + thread.join(timeout=timeout) + self._thread = None + if self._mode == "master": + try: + self._g.exit_zero_gravity() + except Exception as e: # noqa: BLE001 + log.debug("exit_zero_gravity on stop failed: %s", e) + log.info("teleop stopped: mode=%s frames=%d", self._mode, self._frames) + + def status(self) -> dict: + """A snapshot of the session, for logging and diagnostics.""" + age_ms = None + if self._mode == "slave" and self._last_frame_ts > 0.0: + age_ms = (self._time_fn() - self._last_frame_ts) * 1000.0 + return { + "active": self._running, + "mode": self._mode, + "topic": self._topic, + "frames": self._frames, + "last_frame_age_ms": age_ms, + "stale": self._stale, + "openness": round(self._last_openness, 4), + "loop_hz": round(self._loop_hz, 1), + } + + # ── master ──────────────────────────────────────────────────────── + + def _master_loop(self) -> None: + log.info("[master] zero-gravity, publishing to %s", self._topic) + try: + while self._running: + t0 = self._time_fn() + # Keep the frame stream alive: the DM motor self-locks a + # "communication loss" fault ~100 ms after frames stop, so a + # zero-torque frame goes out every cycle even though nothing + # is being commanded. + try: + self._g.send_mit_frame(q=0.0, kp=0.0, kd=0.0) + state = self._g.get_state(wait=False) + except Exception: # noqa: BLE001 + log.exception("[master] CAN error; loop exiting") + break + openness = rad_to_openness(state.position_rad, self._g.config) + self._last_openness = openness + try: + self._tp.pub(self._topic, encode_frame( + openness, state.position_mm, state.force_n, + self._time_fn())) + except Exception as e: # noqa: BLE001 + log.debug("[master] publish failed: %s", e) + self._frames += 1 + self._sleep_rest(t0) + finally: + log.info("[master] loop exited (%d frames sent)", self._frames) + + # ── slave ───────────────────────────────────────────────────────── + + def _slave_loop(self) -> None: + transport = self._sub_tp or self._tp + sub = transport.sub(self._topic) + cfg = self._g.config + log.info("[slave] subscribed %s (align=%s watchdog=%.0fms)", + self._topic, self._align, self._watchdog_s * 1000.0) + + # Until a frame arrives, hold wherever the jaws already are. + openness_cmd = rad_to_openness(self._g.get_state(wait=False).position_rad, cfg) + q_cmd = openness_to_rad(openness_cmd, cfg) + + if self._align: + first = self._wait_first_frame(sub, timeout_s=5.0) + if first is not None: + openness_cmd = _clamp01(first[0]) + q_cmd = openness_to_rad(openness_cmd, cfg) + log.info("[slave] aligning to first frame: openness=%.3f -> %.3f rad", + openness_cmd, q_cmd) + try: + self._g.goto_rad(q_cmd, kp=self._resolve_kp(), + kd=self._resolve_kd(), duration=1.0) + except Exception as e: # noqa: BLE001 + log.warning("[slave] align goto_rad failed: %s", e) + self._last_frame_ts = self._time_fn() + else: + log.warning("[slave] no frame within align timeout; " + "holding current position") + + try: + while self._running: + t0 = self._time_fn() + msg = sub.drain_latest() + if msg is not None: + try: + openness, _mm, _force, _ts = decode_frame(msg) + except ValueError as e: + log.debug("[slave] ignoring bad frame: %s", e) + else: + openness_cmd = _clamp01(openness) + q_cmd = openness_to_rad(openness_cmd, cfg) + self._last_openness = openness_cmd + self._last_frame_ts = self._time_fn() + self._frames += 1 + self._stale = False + elif (self._last_frame_ts > 0.0 + and (self._time_fn() - self._last_frame_ts) > self._watchdog_s): + if not self._stale: + log.warning("[slave] frames stale (>%.0fms); holding position", + self._watchdog_s * 1000.0) + self._stale = True + + # Always send — including while stale. The frame both holds + # the position and keeps the motor from self-locking. + try: + self._g.send_mit_frame(q=q_cmd, kp=self._resolve_kp(), + kd=self._resolve_kd(), dq=0.0) + except Exception: # noqa: BLE001 + log.exception("[slave] CAN error; loop exiting") + break + self._sleep_rest(t0) + finally: + log.info("[slave] loop exited (%d frames received)", self._frames) + + # ── helpers ─────────────────────────────────────────────────────── + + def _wait_first_frame(self, sub: TeleopSubscription, + timeout_s: float) -> Optional[Tuple[float, float, float, float]]: + deadline = self._time_fn() + timeout_s + while self._running and self._time_fn() < deadline: + msg = sub.drain_latest() + if msg is not None: + try: + return decode_frame(msg) + except ValueError: + continue + self._sleep_fn(0.01) + return None + + def _resolve_kp(self) -> float: + return self._kp if self._kp is not None else 100.0 + + def _resolve_kd(self) -> float: + return self._kd if self._kd is not None else 2.0 + + def _sleep_rest(self, t0: float) -> None: + self._loops += 1 + now = self._time_fn() + if self._hz_t0 == 0.0: + self._hz_t0 = now + self._hz_n0 = self._loops + elif now - self._hz_t0 >= 1.0: + self._loop_hz = (self._loops - self._hz_n0) / (now - self._hz_t0) + self._hz_t0 = now + self._hz_n0 = self._loops + rest = self._dt - (self._time_fn() - t0) + if rest > 0.0: + self._sleep_fn(rest) diff --git a/tests/test_teleop.py b/tests/test_teleop.py new file mode 100644 index 0000000..dea3ab7 --- /dev/null +++ b/tests/test_teleop.py @@ -0,0 +1,296 @@ +"""Hardware-free tests for leader/follower gripper teleoperation. + +The loops run for real (a thread each) but with the sleep seam stubbed, so a +"cycle" costs microseconds. Assertions therefore poll with a deadline rather +than assuming a fixed number of cycles elapsed. + +The slave-side tests publish through :class:`_PreSubTransport`, whose +subscription exists before the loop starts. A real ``InProcTeleopTransport`` +only delivers to subscribers that already exist, so publishing before +``start()`` would otherwise race the loop's own ``sub()`` call. +""" + +from __future__ import annotations + +import time +import unittest + +import _sdkpath # noqa: F401 +from litegrip import (FRAME_SIZE, GripperTeleop, InProcTeleopTransport, + TeleopBusyError, UdpTeleopTransport, decode_frame, + encode_frame, teleop_topic) +from litegrip.teleop import openness_to_rad, rad_to_openness, travel_mm + +from fake_can import POS_CLOSED_RAD, POS_OPEN_RAD, RAD_TO_MM, make_gripper + +TOPIC = teleop_topic("master") + +# Far longer than the stub-sleep loop needs; a timeout here means the loop is +# not running at all, not that the machine is slow. +WAIT_S = 2.0 + + +def _wait_until(predicate, timeout_s: float = WAIT_S) -> bool: + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + if predicate(): + return True + time.sleep(0.001) + return predicate() + + +def _nop_sleep(_seconds: float) -> None: + """Stub the loop's pacing so tests do not pay the frame interval.""" + + +class _PreSubTransport(InProcTeleopTransport): + """In-process bus with the slave's subscription created up front.""" + + def __init__(self) -> None: + super().__init__() + self.handle = super().sub(TOPIC) + + def sub(self, topic: str): + return self.handle if topic == TOPIC else super().sub(topic) + + +class FrameCodecTest(unittest.TestCase): + def test_size_is_four_doubles(self): + self.assertEqual(FRAME_SIZE, 32) + self.assertEqual(len(encode_frame(0.0, 0.0, 0.0, 0.0)), FRAME_SIZE) + + def test_roundtrip(self): + payload = encode_frame(0.25, 61.5, -3.5, 1234.5) + self.assertEqual(decode_frame(payload), (0.25, 61.5, -3.5, 1234.5)) + + def test_decode_rejects_wrong_size(self): + with self.assertRaises(ValueError): + decode_frame(b"\x00" * (FRAME_SIZE - 1)) + + +class ConversionTest(unittest.TestCase): + def test_travel_matches_limits(self): + g, _ = make_gripper() + self.assertAlmostEqual( + travel_mm(g.config), abs(POS_CLOSED_RAD - POS_OPEN_RAD) * RAD_TO_MM) + + def test_normal_mount_endpoints_and_inverse(self): + g, _ = make_gripper(reverse=False) + cfg = g.config + self.assertAlmostEqual(openness_to_rad(0.0, cfg), cfg.pos_closed_rad) + self.assertAlmostEqual(openness_to_rad(1.0, cfg), cfg.pos_open_rad) + self.assertAlmostEqual(rad_to_openness(cfg.pos_closed_rad, cfg), 0.0) + self.assertAlmostEqual(rad_to_openness(cfg.pos_open_rad, cfg), 1.0) + # Round trip, and agreement with the SDK's own goto_mm formula. + self.assertAlmostEqual(rad_to_openness(openness_to_rad(0.37, cfg), cfg), + 0.37, places=6) + self.assertAlmostEqual( + openness_to_rad(0.4, cfg), + cfg.pos_closed_rad - 0.4 * travel_mm(cfg) / cfg.rad_to_mm) + + def test_reverse_mount_endpoints_and_inverse(self): + g, _ = make_gripper(reverse=True) + cfg = g.config + self.assertEqual(cfg.close_sign, -1.0) + self.assertAlmostEqual(openness_to_rad(0.0, cfg), cfg.pos_closed_rad) + self.assertAlmostEqual(openness_to_rad(1.0, cfg), cfg.pos_open_rad) + self.assertAlmostEqual(rad_to_openness(cfg.pos_open_rad, cfg), 1.0) + self.assertAlmostEqual(rad_to_openness(openness_to_rad(0.6, cfg), cfg), + 0.6, places=6) + # Reverse is the normal formula with the sign flipped — the fix over + # the litearm original, which assumed a normal mount. + self.assertAlmostEqual( + openness_to_rad(0.4, cfg), + cfg.pos_closed_rad + 0.4 * travel_mm(cfg) / cfg.rad_to_mm) + + def test_openness_is_clamped(self): + g, _ = make_gripper() + cfg = g.config + self.assertAlmostEqual(openness_to_rad(-5.0, cfg), openness_to_rad(0.0, cfg)) + self.assertAlmostEqual(openness_to_rad(5.0, cfg), openness_to_rad(1.0, cfg)) + + +class InProcTransportTest(unittest.TestCase): + def test_pub_sub_drain_latest(self): + transport = InProcTeleopTransport() + sub = transport.sub("t") + transport.pub("t", b"old") + transport.pub("t", b"new") + self.assertEqual(sub.drain_latest(), b"new") + self.assertIsNone(sub.drain_latest()) + + def test_bounded_queue_drops_oldest(self): + transport = InProcTeleopTransport(fifo_depth=2) + sub = transport.sub("t") + for i in range(5): + transport.pub("t", bytes([i])) + self.assertEqual(sub.drain_latest(), bytes([4])) + self.assertIsNone(sub.try_recv()) + + +class UdpTransportTest(unittest.TestCase): + def test_loopback_roundtrip(self): + receiver = UdpTeleopTransport(bind_addr=("127.0.0.1", 0)) + port = receiver._sock.getsockname()[1] + sender = UdpTeleopTransport(pub_addr=("127.0.0.1", port)) + sub = receiver.sub(TOPIC) + try: + sender.pub(TOPIC, b"frame") + self.assertTrue(_wait_until(lambda: sub.drain_latest() == b"frame")) + finally: + sender.close() + receiver.close() + + def test_requires_an_address(self): + with self.assertRaises(ValueError): + UdpTeleopTransport() + + +class MasterLoopTest(unittest.TestCase): + def test_publishes_openness_and_zero_torque(self): + g, fake = make_gripper(start_rad=POS_OPEN_RAD) + # The fake motor is purely kinematic: it tracks ``q`` regardless of gain, + # so the master's zero-torque ``q=0`` command would drag it closed. A + # real slack jaw does not move under zero gain, so freeze the fake to + # model that — the opening published is then where the jaws started. + fake.motor.step = lambda *a, **k: None + transport = InProcTeleopTransport() + sub = transport.sub(TOPIC) + mgr = GripperTeleop(g, transport, "master", TOPIC, rate_hz=200.0, + sleep_fn=_nop_sleep) + mgr.start() + try: + self.assertTrue(_wait_until(lambda: mgr.status()["frames"] > 0)) + openness, position_mm, _force, _ts = decode_frame(sub.drain_latest()) + self.assertAlmostEqual(openness, 1.0, places=3) + self.assertAlmostEqual(position_mm, travel_mm(g.config), places=3) + last = fake.frames[-1] + self.assertEqual((last.kp, last.kd), (0.0, 0.0)) + finally: + mgr.stop() + + def test_stop_leaves_zero_gravity(self): + g, fake = make_gripper() + mgr = GripperTeleop(g, InProcTeleopTransport(), "master", TOPIC, + rate_hz=200.0, sleep_fn=_nop_sleep) + mgr.start() + self.assertTrue(_wait_until(lambda: len(fake.frames) > 0)) + mgr.stop() + self.assertFalse(mgr.is_running) + # The final frame is not zero-torque: exit_zero_gravity holds instead. + self.assertEqual(fake.frames[-1].kp, g.config.kp) + self.assertFalse(mgr.status()["active"]) + + +class SlaveLoopTest(unittest.TestCase): + def _slave(self, **kwargs): + g, fake = make_gripper(start_rad=POS_CLOSED_RAD, reverse=False) + transport = _PreSubTransport() + mgr = GripperTeleop(g, transport, "slave", TOPIC, rate_hz=200.0, + sleep_fn=_nop_sleep, **kwargs) + return g, fake, transport, mgr + + def test_follows_published_openness(self): + g, fake, transport, mgr = self._slave(align=False, watchdog_s=5.0) + transport.pub(TOPIC, encode_frame(1.0, 120.0, 0.0, 0.0)) + target = openness_to_rad(1.0, g.config) + mgr.start() + try: + self.assertTrue(_wait_until( + lambda: fake.frames and abs(fake.frames[-1].q - target) < 1e-9)) + last = fake.frames[-1] + self.assertEqual((last.kp, last.kd), (100.0, 2.0)) + self.assertFalse(mgr.status()["stale"]) + finally: + mgr.stop() + + def test_watchdog_holds_instead_of_relaxing(self): + g, fake, transport, mgr = self._slave(align=False, watchdog_s=0.05) + transport.pub(TOPIC, encode_frame(0.8, 96.0, 0.0, 0.0)) + target = openness_to_rad(0.8, g.config) + mgr.start() + try: + self.assertTrue(_wait_until( + lambda: fake.frames and abs(fake.frames[-1].q - target) < 1e-9)) + # Silence the leader and let the watchdog trip. + self.assertTrue(_wait_until(lambda: mgr.status()["stale"])) + before = len(fake.frames) + self.assertTrue(_wait_until(lambda: len(fake.frames) > before + 5)) + # Still streaming, still on target, still under gain. + self.assertAlmostEqual(fake.frames[-1].q, target) + self.assertEqual(fake.frames[-1].kp, 100.0) + finally: + mgr.stop() + + def test_align_uses_first_frame_before_following(self): + g, fake, transport, mgr = self._slave(align=True, watchdog_s=5.0) + # Queued before start; the align step must be the first CAN traffic. + transport.pub(TOPIC, encode_frame(0.5, 60.0, 0.0, 0.0)) + expected = openness_to_rad(0.5, g.config) + mgr.start() + try: + self.assertTrue(_wait_until(lambda: len(fake.frames) > 0)) + self.assertAlmostEqual(fake.frames[0].q, expected) + finally: + mgr.stop() + + def test_ignores_malformed_payload(self): + g, fake, transport, mgr = self._slave(align=False, watchdog_s=5.0) + transport.pub(TOPIC, b"too short") + transport.pub(TOPIC, encode_frame(0.25, 30.0, 0.0, 0.0)) + target = openness_to_rad(0.25, g.config) + mgr.start() + try: + self.assertTrue(_wait_until( + lambda: fake.frames and abs(fake.frames[-1].q - target) < 1e-9)) + finally: + mgr.stop() + + +class LiteGripTeleopApiTest(unittest.TestCase): + def test_status_idle(self): + g, _ = make_gripper() + self.assertEqual(g.teleop_status(), {"active": False, "mode": None}) + + def test_stop_when_idle_is_harmless(self): + g, _ = make_gripper() + self.assertEqual(g.teleop_stop(), {"active": False, "mode": None}) + + def test_start_then_busy_then_stop(self): + g, _ = make_gripper() + transport = InProcTeleopTransport() + status = g.teleop_start("slave", transport=transport, align=False) + self.assertTrue(status["active"]) + self.assertEqual(status["mode"], "slave") + try: + with self.assertRaises(TeleopBusyError): + g.teleop_start("slave", transport=transport, align=False) + finally: + stopped = g.teleop_stop() + self.assertFalse(stopped["active"]) + self.assertEqual(g.teleop_status(), {"active": False, "mode": None}) + + def test_rejects_unknown_mode(self): + g, _ = make_gripper() + with self.assertRaises(ValueError): + g.teleop_start("sideways", transport=InProcTeleopTransport()) + + def test_start_discovers_udp_transport_and_closes_it(self): + g, _ = make_gripper() + g.teleop_start("slave", host="127.0.0.1", port=0, align=False, + rate_hz=200.0) + self.assertIsNotNone(g._teleop_transport) + self.assertIsNotNone(g._teleop_transport._sock) + g.teleop_stop() + self.assertIsNone(g._teleop_transport) + + def test_disconnect_stops_teleop(self): + g, _ = make_gripper() + g.teleop_start("slave", transport=InProcTeleopTransport(), align=False) + self.assertIsNotNone(g._teleop) + g.disconnect() + self.assertIsNone(g._teleop) + + +if __name__ == "__main__": + unittest.main()