feat: run gripper teleop over the validated zenoh link

README.md

@@ -17,6 +17,7 @@ using the MIT control protocol.
 | Platform | Linux only (SocketCAN) |
 | Python | 3.8 or newer |
 | Runtime dependencies | none, standard library only |
+| Optional extra | `litegrip[zenoh]` — the point-to-point zenoh teleoperation link |
 
 ## Installation
 
@@ -117,20 +118,39 @@ you push its jaws by hand — and it publishes how far open it is at the loop ra
 normalized opening in `[0, 1]`, not an angle, so the two ends do not need the same calibration,
 mount, or zero point.
 
+### Transport
+
+The link is a **point-to-point zenoh** session — the same structure the field teleoperation runs
+on. Both ends use `mode="peer"` with all discovery **off** (no multicast, no gossip), so the only
+way they find each other is an explicit endpoint: the leader listens on a TCP port, the follower
+connects to the leader's address. The topic is the shared litearm namespace,
+`litearm/v4/{grip_id}/gripper_teleop`, and the frame is byte-identical to the litearm stack's, so
+the two interoperate.
+
+zenoh is an optional dependency — the base SDK stays stdlib + SocketCAN:
+
+```bash
+pip install 'litegrip[zenoh]'
+```
+
+`link="udp"` selects a plain-UDP fallback for a trusted LAN; it has no authentication or
+encryption. Pass `transport=` a `TeleopTransport` to supply your own; an injected one is never
+closed by the SDK.
+
 ```python
 from litegrip import LiteGrip
 
-# Leader: publish this gripper's opening to the follower at 192.168.1.20.
+# Leader: listen and publish this gripper's opening.
 with LiteGrip("can0") as master:
     master.load_calibration()
     master.enable()
-    master.teleop_start("master", host="192.168.1.20")
+    master.teleop_start("master")                      # zenoh, gripA, port 17448
 
-# Follower: bind, align to the first frame, then follow.
+# Follower: connect to the leader, 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")
+    slave.teleop_start("slave", host="192.168.1.20")
     while True:
         print(slave.teleop_status())   # frames, openness, loop_hz, stale, ...
 ```
@@ -139,27 +159,34 @@ with LiteGrip("can0") as slave:
 
 ```bash
 # Machine A — the leader you push by hand:
-python3 examples/teleop.py --mode master --channel can0 --host 192.168.1.20
+python3 examples/teleop.py --mode master --channel can0
 # Machine B — the follower:
-python3 examples/teleop.py --mode slave  --channel can0 --host 0.0.0.0
+python3 examples/teleop.py --mode slave  --channel can0 --host 192.168.1.20
 ```
 
-Both ends must share `master_id` (default `master`) and be connected and enabled first. Teleop is
+Both ends must share `grip_id` (default `gripA`) 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`.
+reports `active`, `mode`, `topic`, `frames`, `last_frame_age_ms`, `stale`, `openness`,
+`loop_hz`, `rejected`, `send_failed`, `fault`, and (master) `matching`.
 
-- **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.
+- **Non-finite frames are dropped, never clamped.** A NaN opening would pass a `[0, 1]` clamp and
+  then fold onto a hard stop, silently driving the follower closed. Both ends reject NaN / ±inf at
+  the wire boundary — including the first frame used for the align — count them in `rejected`, and
+  hold position instead.
+- **The follower clamps the target into its own calibrated travel every cycle**, and checks what
+  the SDK tells it: a `send_mit_frame` that returns `False` bumps `send_failed`, and a gripper
+  `error_code` other than "enabled" is reported in `fault` — neither is swallowed.
 - **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=`.
+  `teleop_stop()` and the follower sends one final frame at its current angle, so both hold under
+  the configured gains and neither disables.
+- Teleop refuses to start on an uncalibrated gripper, a zero-travel one, or one with
+  `rad_to_mm == 0` (`TeleopNotReady`), before anything is enabled or driven.
+- Follow gains default to the calibration's `kp` / `kd` (`100.0` / `2.0` out of the box); override
+  with `kp=` / `kd=`.
 
 ## The six actions
 

examples/teleop.py

@@ -5,28 +5,43 @@
 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
+    python3 examples/teleop.py --mode master --channel can0
 
     # Machine B (the follower that copies it):
-    python3 examples/teleop.py --mode slave  --channel can0 --host 0.0.0.0
+    python3 examples/teleop.py --mode slave --channel can0 --host 192.168.1.20
 
-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.
+Both ends must share ``--grip-id``. The default link is the point-to-point zenoh
+transport used by the field teleoperation (``pip install litegrip[zenoh]``): the
+leader listens on ``--port``, the follower connects to ``--host``. ``--link udp``
+selects plain UDP instead, which carries no authentication or encryption — keep
+either on a trusted network. Press Ctrl+C on either end to stop; the gripper
+holds its position and does not disable.
 
 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.
+
+It runs from a source checkout as well as from an installed package: ``src/`` is
+put on the import path below if ``litegrip`` is not installed yet.
 """
 
 from __future__ import annotations
 
 import argparse
+import os
 import sys
 import time
 
-from litegrip import LiteGrip, LiteGripError
+# Like tests/_sdkpath.py: import the SDK straight out of the checkout, so the
+# example works without `pip install -e .`. Inserted first, so the checkout wins
+# over an installed copy — running the example exercises the code next to it.
+_SRC = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "src")
+if _SRC not in sys.path:
+    sys.path.insert(0, _SRC)
+
+from litegrip import (DEFAULT_GRIP_ID, DEFAULT_GRIP_PORT,  # noqa: E402
+                      LiteGrip, LiteGripError)
 
 
 def build_parser() -> argparse.ArgumentParser:
@@ -41,22 +56,26 @@ def build_parser() -> argparse.ArgumentParser:
         "--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")
+        "--link", choices=("zenoh", "udp"), default="zenoh",
+        help="transport (default: zenoh; udp is the plain-LAN fallback)")
+    parser.add_argument(
+        "--host", default=None,
+        help="slave: the leader's address (the master only listens)")
     parser.add_argument(
-        "--port", type=int, default=7448, help="UDP port (default: 7448)")
+        "--port", type=int, default=DEFAULT_GRIP_PORT,
+        help=f"TCP (zenoh) or UDP port (default: {DEFAULT_GRIP_PORT})")
     parser.add_argument(
-        "--master-id", default="master",
-        help="topic id both ends must agree on (default: master)")
+        "--grip-id", default=DEFAULT_GRIP_ID,
+        help=f"topic id both ends must agree on (default: {DEFAULT_GRIP_ID})")
     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)")
+        help="follower stiffness (default: the calibration's kp)")
     parser.add_argument(
         "--kd", type=float, default=None,
-        help="follower damping (default: 2.0)")
+        help="follower damping (default: the calibration's kd)")
     parser.add_argument(
         "--no-align", action="store_true",
         help="follower: skip the one-shot align to the first frame")
@@ -77,10 +96,19 @@ def _print_status(status: dict) -> None:
     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}"
+    extra = ""
+    if status.get("matching") is not None:
+        extra += f" matching={str(status['matching']):>5}"
+    if status.get("rejected"):
+        extra += f" rejected={status['rejected']}"
+    if status.get("send_failed"):
+        extra += f" send_failed={status['send_failed']}"
+    if status.get("fault"):
+        extra += f" fault={status['fault']}"
     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)
+          f"openness={open_txt} "          f"loop_hz={status.get('loop_hz', 0.0):4.1f}"
+          f"{extra}", flush=True)
 
 
 def main(argv: list[str] | None = None) -> int:
@@ -94,18 +122,28 @@ def main(argv: list[str] | None = None) -> int:
     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}")
 
+    target = f"{args.host}:{args.port}" if args.host else f"*:{args.port}"
     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})")
+        print(f"dry run: would start {args.mode} on {args.channel} over "
+              f"{args.link} at {target} "
+              f"(topic litearm/v4/{args.grip_id}/gripper_teleop)")
         return 0
 
     gripper.connect()
-    gripper.enable()
+    # enable() does not raise on failure — it returns a falsy EnableResult.
+    result = gripper.enable()
+    if not result.ok:
+        code = None if result.state is None else result.state.error_code
+        print(f"error: enable failed after {result.tries} tries "
+              f"(last error_code={code}); check the 24V supply and that "
+              f"{args.channel} is up at the right bitrate", file=sys.stderr)
+        gripper.disconnect()
+        return 1
+
     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)
+        args.mode, link=args.link, host=args.host, port=args.port,
+        grip_id=args.grip_id, kp=args.kp, kd=args.kd, align=not args.no_align,
+        watchdog_s=args.watchdog, rate_hz=args.rate)
     print(f"teleop {args.mode} running; Ctrl+C to stop")
     _print_status(status)
 

pyproject.toml

@@ -50,6 +50,12 @@ classifiers = [
 # that this installs on a bare robot controller.
 dependencies = []
 
+[project.optional-dependencies]
+# The point-to-point zenoh link used by the field teleoperation.  Optional so
+# the base install stays stdlib + SocketCAN; only `litegrip.zenoh_link` and the
+# default `teleop_start` transport need it.
+zenoh = ["eclipse-zenoh>=1.0"]
+
 [project.urls]
 Repository = "https://github.com/nexform-tech/litegrip-python"
 Issues = "https://github.com/nexform-tech/litegrip-python/issues"

readme_zn.md

@@ -16,6 +16,7 @@
 | 平台 | 仅 Linux（SocketCAN） |
 | Python | 3.8 及以上 |
 | 运行时依赖 | 无，只用标准库 |
+| 可选扩展 | `litegrip[zenoh]` —— 点对点 zenoh 遥操链路 |
 
 ## 安装
 
@@ -104,20 +105,36 @@ with LiteGrip("can1", mount="reverse") as gripper:
 它按循环频率把「张开程度」发出去；**从夹爪**（follower）收到后驱动自己的爪子跟到位。线上传的
 是归一化到 `[0, 1]` 的张开度，不是角度，所以两端不需要相同的标定、装法或零点。
 
+### 传输
+
+链路是**点对点 zenoh** —— 与真机上跑的遥操同一套结构。两端都是 `mode="peer"`，**关掉全部
+发现机制**（无多播、无 gossip），所以两端只能靠显式端点互相找到：主端监听一个 TCP 端口，从端
+连到主端的地址。话题是共用的 litearm 命名空间 `litearm/v4/{grip_id}/gripper_teleop`，帧格式
+与 litearm 那套逐字节相同 ⇒ 两边可以互通。
+
+zenoh 是**可选依赖**，基础 SDK 仍是「标准库 + SocketCAN」：
+
+```bash
+pip install 'litegrip[zenoh]'
+```
+
+`link="udp"` 可切到明文 UDP 回退方案（仅限可信局域网，无鉴权、无加密）。要给自定义传输，传
+`transport=` 一个 `TeleopTransport`；注入的传输不会被 SDK 关闭。
+
 ```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")
+    master.teleop_start("master")                      # zenoh，gripA，端口 17448
 
-# 从端：绑定端口，先对齐首帧，然后跟随。
+# 从端：连到主端，先对齐首帧，然后跟随。
 with LiteGrip("can0") as slave:
     slave.load_calibration()
     slave.enable()
-    slave.teleop_start("slave", host="0.0.0.0")
+    slave.teleop_start("slave", host="192.168.1.20")
     while True:
         print(slave.teleop_status())   # frames, openness, loop_hz, stale, ...
 ```
@@ -126,24 +143,30 @@ with LiteGrip("can0") as slave:
 
 ```bash
 # A 机 —— 你用手掰的主夹爪：
-python3 examples/teleop.py --mode master --channel can0 --host 192.168.1.20
+python3 examples/teleop.py --mode master --channel can0
 # B 机 —— 从夹爪：
-python3 examples/teleop.py --mode slave  --channel can0 --host 0.0.0.0
+python3 examples/teleop.py --mode slave  --channel can0 --host 192.168.1.20
 ```
 
-两端必须共用 `master_id`（默认 `master`），且都已连接、已使能。遥操是互斥的：后台循环独占 CAN
+两端必须共用 `grip_id`（默认 `gripA`），且都已连接、已使能。遥操是互斥的：后台循环独占 CAN
 读写，在 `teleop_stop()` 之前不要再从调用方驱动夹爪。`teleop_start` 返回初始的
 `teleop_status()`；`teleop_status()` 报告 `active`、`mode`、`topic`、`frames`、
-`last_frame_age_ms`、`stale`、`openness`、`loop_hz`。
+`last_frame_age_ms`、`stale`、`openness`、`loop_hz`、`rejected`、`send_failed`、
+`fault`，主端另有 `matching`。
 
-- **传输是明文 UDP**，无鉴权、无加密，只用在可信网络里。要给自定义传输，传 `transport=` 一个
-  `TeleopTransport`；注入的传输不会被 SDK 关闭。
 - **从端与主端失联时是「持位」，不是「卸力」。** 超过 `watchdog_s`（默认 `0.2`）没有新帧后，
   它仍按跟随增益顶着上一个目标继续发帧 —— 于是 `stale` 变真，但爪子停在原地，可能夹住中间的
   东西。
-- **从端会把收到的张开度夹到 `[0, 1]`**，也就是夹在自己的标定行程内，坏帧无法把它指到限位之外。
-- **停止后是持位**，不是卸力：主端在 `teleop_stop()` 时退出零重力模式，爪子按配置增益持位。
-- 跟随增益默认 `kp=100.0`、`kd=2.0`，用 `kp=` / `kd=` 覆盖。
+- **非有限值帧一律丢弃，绝不夹位。** NaN 会原样穿过 `[0, 1]` 的钳位，再被折到某个端点 —— 静默
+  地把从端指到全闭限位。两端都在**协议边界**上拒收 NaN / ±inf（包括对齐用的首帧），计入
+  `rejected`，并改为持位。
+- **从端每拍都把目标夹进自己的标定行程**，并且会看 SDK 的返回值：`send_mit_frame` 返回 `False`
+  会计入 `send_failed`，夹爪自报的 `error_code` 不是「已使能」会记进 `fault` —— 都不吞掉。
+- **停止后是持位**，不是卸力：主端在 `teleop_stop()` 时退出零重力模式，从端在收尾时按当前角度
+  补发一帧 —— 两边都按配置增益持位，都不失能。
+- 未标定、行程为零、或 `rad_to_mm == 0` 的夹爪会**拒绝启动**（`TeleopNotReady`），且在使能或
+  驱动之前就拒掉。
+- 跟随增益默认取标定里的 `kp` / `kd`（出厂是 `100.0` / `2.0`），用 `kp=` / `kd=` 覆盖。
 
 ## 六个动作接口