From a64f089cc5f4a725805b11aee47cb18033c9bf11 Mon Sep 17 00:00:00 2001 From: yd-sl <166189879+yd-sl@users.noreply.github.com> Date: Thu, 12 Nov 2026 13:59:12 +0800 Subject: [PATCH 1/2] feat: hold the grasp on over-torque and return the force to the leader MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit An over-torque follower used to drop to zero stiffness and damping in place, so the clamped part pushed the jaws straight open — the gripper could not hold anything. It now keeps pressing with a bounded pure-torque frame (kp=kd=0, tau=+/-torque_limit_nm, signed by the torque measured at the trip), the same shape as the hardware-validated GripperActions._hold_force. The old behaviour is still available as torque_guard_action="release". The leader can now feel the grasp: the follower publishes its mount-normalised squeeze on its own zenoh key (litearm/v4/{grip_id}/gripper_feedback, an 8-byte double, additive so older peers degrade to position-only teleop) and the leader renders a resistance torque above a threshold, capped, with damping only while the force is fresh. The forward and ready frames are untouched. UDP has no receive path on the leader, so feedback turns itself off there. Both mounts are covered by tests: the hold direction follows the measured torque sign, not close_sign, and a wrong leader sign would drive the grasp closed instead of resisting it. --- README.md | 26 ++- README.zh-CN.md | 20 +- src/litegrip/__init__.py | 22 +++ src/litegrip/gripper.py | 67 +++++-- src/litegrip/teleop.py | 391 +++++++++++++++++++++++++++++++++---- src/litegrip/zenoh_link.py | 82 ++++++-- tests/test_teleop.py | 363 +++++++++++++++++++++++++++++++--- 7 files changed, 862 insertions(+), 109 deletions(-) diff --git a/README.md b/README.md index 3e4ba9d..a9590bf 100644 --- a/README.md +++ b/README.md @@ -513,9 +513,9 @@ with LiteGrip("can0") as slave: | Method | What it does | | --- | --- | -| `teleop_start(mode, *, transport=None, link="zenoh", host=None, port=17448, grip_id="gripA", kp=None, kd=None, align=True, watchdog_s=0.2, dq_max=10.0, torque_limit_nm=0.0, require_ready=True, ...)` → `dict` | Starts the background loop for `mode="master"` or `"slave"`. Both ends must share `grip_id` and be connected and enabled first. | +| `teleop_start(mode, *, transport=None, link="zenoh", host=None, port=17448, grip_id="gripA", kp=None, kd=None, align=True, watchdog_s=0.2, dq_max=10.0, torque_limit_nm=0.0, torque_guard_action="hold", require_ready=True, force_feedback=False, feedback_gain_nm_per_n=0.10, ...)` → `dict` | Starts the background loop for `mode="master"` or `"slave"`. Both ends must share `grip_id` and be connected and enabled first. | | `teleop_stop(timeout=2.0)` → `dict` | Stops the loop and leaves the jaws holding. | -| `teleop_status()` → `dict` | `active`, `mode`, `frames`, `last_frame_age_ms`, `stale`, `openness`, `position_mm`, `force_n`, `loop_hz`, `rejected`, `send_failed`, `fault`, `torque_nm`, `over_torque`, `torque_trips`, `ready`, and more. | +| `teleop_status()` → `dict` | `active`, `mode`, `frames`, `last_frame_age_ms`, `stale`, `openness`, `position_mm`, `force_n`, `loop_hz`, `rejected`, `send_failed`, `fault`, `torque_nm`, `over_torque`, `torque_trips`, `ready`, `feedback_force_n`, `feedback_tau_nm`, `feedback_rx`, `feedback_tx`, and more. | The link is a point-to-point **zenoh** session, an optional dependency: @@ -545,11 +545,23 @@ python3 examples/teleop.py --mode slave --channel can0 --fake-leader --torque-li - **The leader holds its jaws under gain until the follower reports ready**, so the operator cannot push the target out from under a follower that is still travelling. `ready_timeout_s` (default `10.0`) bounds the wait; `require_ready=False` restores the immediate hand-back. -- **An optional torque guard releases a follower that is pressing too hard.** Set - `torque_limit_nm` and it goes to zero stiffness and damping *in place* after 60 ms over the - limit, then re-arms once the leader has reopened. The default is `0` — **the guard is off - unless you ask for it**. Pick the value per machine: it depends on how fragile the part between - the jaws is. +- **An optional torque guard stops a follower that is pressing too hard.** Set + `torque_limit_nm` and, after 60 ms over the limit, the follower stops following and **keeps + pressing with that torque** (`kp=kd=0`, `tau=±torque_limit_nm`, signed by the torque measured + at the trip) so a clamped part is not pushed open; it re-arms once the leader has reopened. + `torque_guard_action="release"` restores the previous behaviour — zero stiffness and damping + *in place* — which lets the part push the jaws straight open. The default is `0` — **the guard + is off unless you ask for it**. Pick the value per machine: it depends on how fragile the part + between the jaws is. +- **The follower's squeeze can be felt on the leader.** With `force_feedback=True` the follower + publishes `force_n` on `litearm/v4/{grip_id}/gripper_feedback` (positive means closing, + normalised for the mount) and the leader renders resistance at + `min(max(0, force_n - feedback_threshold_n) * feedback_gain_nm_per_n, feedback_max_tau_nm)`, + directed against closing. Defaults: threshold `2.0` N, gain `0.10` Nm/N, cap `1.0` Nm. + Damping (`0.1`) applies only while the force is fresh; torque and damping both drop to zero + after `feedback_timeout_s` (`0.3`). It is off by default and needs the zenoh link — a UDP + leader is publish-only, so it runs without feedback. The key is additive, so an older peer + simply runs position-only teleoperation. - **A follower that loses the leader holds its position, it does not go slack.** After `watchdog_s` (default `0.2`) without a fresh frame, `stale` goes true and the jaws stay put. - **The follower feeds the leader's velocity forward** by differencing successive frames, so it diff --git a/README.zh-CN.md b/README.zh-CN.md index cb28877..58e8ad7 100644 --- a/README.zh-CN.md +++ b/README.zh-CN.md @@ -446,9 +446,9 @@ with LiteGrip("can0") as slave: | 方法 | 做什么 | | --- | --- | -| `teleop_start(mode, *, transport=None, link="zenoh", host=None, port=17448, grip_id="gripA", kp=None, kd=None, align=True, watchdog_s=0.2, dq_max=10.0, torque_limit_nm=0.0, require_ready=True, ...)` → `dict` | 为 `mode="master"` 或 `"slave"` 起后台循环。两端必须共用 `grip_id`,并且都已连接、已使能。 | +| `teleop_start(mode, *, transport=None, link="zenoh", host=None, port=17448, grip_id="gripA", kp=None, kd=None, align=True, watchdog_s=0.2, dq_max=10.0, torque_limit_nm=0.0, torque_guard_action="hold", require_ready=True, force_feedback=False, feedback_gain_nm_per_n=0.10, ...)` → `dict` | 为 `mode="master"` 或 `"slave"` 起后台循环。两端必须共用 `grip_id`,并且都已连接、已使能。 | | `teleop_stop(timeout=2.0)` → `dict` | 停掉循环,爪子保持在当前位置。 | -| `teleop_status()` → `dict` | `active`、`mode`、`frames`、`last_frame_age_ms`、`stale`、`openness`、`position_mm`、`force_n`、`loop_hz`、`rejected`、`send_failed`、`fault`、`torque_nm`、`over_torque`、`torque_trips`、`ready` 等。 | +| `teleop_status()` → `dict` | `active`、`mode`、`frames`、`last_frame_age_ms`、`stale`、`openness`、`position_mm`、`force_n`、`loop_hz`、`rejected`、`send_failed`、`fault`、`torque_nm`、`over_torque`、`torque_trips`、`ready`、`feedback_force_n`、`feedback_tau_nm`、`feedback_rx`、`feedback_tx` 等。 | 链路是**点对点 zenoh**,属于可选依赖: @@ -476,9 +476,19 @@ python3 examples/teleop.py --mode slave --channel can0 --fake-leader --torque-li - **从端报告就位之前,主端一直带着增益顶着自己的爪子**,所以操作者无法在一个还在移动的从端 下面把目标推走。等待由 `ready_timeout_s`(默认 `10.0`)兜底;`require_ready=False` 恢复 立即交还。 -- **可选的力矩护栏会放掉压得太狠的从端。** 设了 `torque_limit_nm` 之后,超过限值 60 ms 它就 - **就地**把刚度和阻尼降到零,等主端重新张开后重新武装。默认是 `0` —— **不显式要求就不启用**。 - 这个值要按机器挑:它取决于爪子之间那个工件有多脆。 +- **可选的力矩护栏会拦住压得太狠的从端。** 设了 `torque_limit_nm` 之后,超过限值 60 ms 它就 + 停止跟随,**改用该限值继续压住**(`kp=kd=0`、`tau=±torque_limit_nm`,方向取触发瞬间实测 + 力矩的符号),被夹住的工件因此不会被钳口顶开;等主端重新张开后重新武装。 + `torque_guard_action="release"` 恢复旧行为 —— **就地**把刚度和阻尼降到零,工件会直接把 + 钳口顶开。默认是 `0` —— **不显式要求就不启用**。这个值要按机器挑:它取决于爪子之间那个 + 工件有多脆。 +- **从端的夹持力可以让主端感觉到。** 打开 `force_feedback=True` 后,从端在 + `litearm/v4/{grip_id}/gripper_feedback` 上发布 `force_n`(正 = 夹紧,已按安装方向归一化), + 主端按 `min(max(0, force_n - feedback_threshold_n) * feedback_gain_nm_per_n, + feedback_max_tau_nm)` 渲染反向阻力。默认阈值 `2.0` N、增益 `0.10` Nm/N、封顶 `1.0` Nm; + 阻尼(`0.1`)只在收到新鲜力时生效,超过 `feedback_timeout_s`(`0.3`)后力矩和阻尼一起归零。 + 该功能默认关闭,且依赖 zenoh 链路 —— UDP 主端只能发不能收,所以自动没有反馈。新增的 key + 是附加的,旧版本对端会自动退化为纯位置遥操。 - **跟丢主端的从端保持原位,不会卸力。** 超过 `watchdog_s`(默认 `0.2`)没有新帧,`stale` 置真、爪子停在原处。 - **从端把主端的速度前馈进去**(对相邻两帧做差分),所以不会拖在移动的主端后面。`dq_max=0` diff --git a/src/litegrip/__init__.py b/src/litegrip/__init__.py index 461fe5d..ec8ed2a 100644 --- a/src/litegrip/__init__.py +++ b/src/litegrip/__init__.py @@ -144,14 +144,25 @@ def _detect_version(dist_name: str = "litegrip-python") -> str: DEFAULT_READY_TOLERANCE_MM, DEFAULT_READY_PERIOD_S, DEFAULT_READY_TIMEOUT_S, + DEFAULT_FEEDBACK_GAIN_NM_PER_N, + DEFAULT_FEEDBACK_THRESHOLD_N, + DEFAULT_FEEDBACK_MAX_TAU_NM, + DEFAULT_FEEDBACK_DAMPING, + DEFAULT_FEEDBACK_TIMEOUT_S, + TORQUE_GUARD_HOLD, + TORQUE_GUARD_RELEASE, FRAME_SIZE, READY_FRAME_SIZE, + FEEDBACK_FRAME_SIZE, encode_frame, decode_frame, encode_ready_frame, decode_ready_frame, + encode_feedback_frame, + decode_feedback_frame, teleop_topic, ready_topic, + feedback_topic, ) # ── Trajectory record and replay ──────────────────────────────────────── @@ -268,14 +279,25 @@ def __dir__(): "DEFAULT_READY_TOLERANCE_MM", "DEFAULT_READY_PERIOD_S", "DEFAULT_READY_TIMEOUT_S", + "DEFAULT_FEEDBACK_GAIN_NM_PER_N", + "DEFAULT_FEEDBACK_THRESHOLD_N", + "DEFAULT_FEEDBACK_MAX_TAU_NM", + "DEFAULT_FEEDBACK_DAMPING", + "DEFAULT_FEEDBACK_TIMEOUT_S", + "TORQUE_GUARD_HOLD", + "TORQUE_GUARD_RELEASE", "FRAME_SIZE", "READY_FRAME_SIZE", + "FEEDBACK_FRAME_SIZE", "encode_frame", "decode_frame", "encode_ready_frame", "decode_ready_frame", + "encode_feedback_frame", + "decode_feedback_frame", "teleop_topic", "ready_topic", + "feedback_topic", # Teleoperation — zenoh link (resolved lazily; needs litegrip-python[zenoh]) "ZenohTeleopTransport", "Listener", diff --git a/src/litegrip/gripper.py b/src/litegrip/gripper.py index dd71325..ab973c9 100644 --- a/src/litegrip/gripper.py +++ b/src/litegrip/gripper.py @@ -47,14 +47,19 @@ _check_calibrated, clamp_to_travel, ) -from .teleop import (DEFAULT_ALIGN_SPEED_MM_S, DEFAULT_DQ_MAX, DEFAULT_GRIP_ID, +from .teleop import (DEFAULT_ALIGN_SPEED_MM_S, DEFAULT_DQ_MAX, + DEFAULT_FEEDBACK_DAMPING, DEFAULT_FEEDBACK_GAIN_NM_PER_N, + DEFAULT_FEEDBACK_MAX_TAU_NM, DEFAULT_FEEDBACK_THRESHOLD_N, + DEFAULT_FEEDBACK_TIMEOUT_S, DEFAULT_GRIP_ID, DEFAULT_GRIP_PORT, DEFAULT_LEAD_CAP_MM, DEFAULT_READY_PERIOD_S, DEFAULT_READY_TIMEOUT_S, - DEFAULT_READY_TOLERANCE_MM, DEFAULT_TORQUE_LIMIT_NM) + DEFAULT_READY_TOLERANCE_MM, DEFAULT_TORQUE_LIMIT_NM, + TORQUE_GUARD_HOLD) def _zenoh_transport(role: str, key: str, port: int, host: Optional[str], - ready_key: Optional[str] = None) -> "TeleopTransport": + ready_key: Optional[str] = None, + feedback_key: Optional[str] = None) -> "TeleopTransport": """Build a point-to-point zenoh transport, or explain how to get one. zenoh is an optional dependency, so a bare ``import`` failure is turned into @@ -68,7 +73,7 @@ def _zenoh_transport(role: str, key: str, port: int, host: Optional[str], "the zenoh teleoperation link needs the optional zenoh dependency — " "install it with `pip install litegrip-python[zenoh]`") from e return ZenohTeleopTransport(role, key, port=port, host=host, - ready_key=ready_key) + ready_key=ready_key, feedback_key=feedback_key) # The calibration the SDK ships for the unit it ships with. This is the @@ -2193,11 +2198,18 @@ def teleop_start( watchdog_s: float = 0.2, dq_max: float = DEFAULT_DQ_MAX, torque_limit_nm: float = DEFAULT_TORQUE_LIMIT_NM, + torque_guard_action: str = TORQUE_GUARD_HOLD, lead_cap_mm: float = DEFAULT_LEAD_CAP_MM, # [遥操对齐块] require_ready: bool = True, ready_timeout_s: float = DEFAULT_READY_TIMEOUT_S, ready_tolerance_mm: float = DEFAULT_READY_TOLERANCE_MM, ready_period_s: float = DEFAULT_READY_PERIOD_S, + force_feedback: bool = False, + feedback_gain_nm_per_n: float = DEFAULT_FEEDBACK_GAIN_NM_PER_N, + feedback_threshold_n: float = DEFAULT_FEEDBACK_THRESHOLD_N, + feedback_max_tau_nm: float = DEFAULT_FEEDBACK_MAX_TAU_NM, + feedback_damping: float = DEFAULT_FEEDBACK_DAMPING, + feedback_timeout_s: float = DEFAULT_FEEDBACK_TIMEOUT_S, rate_hz: float = 50.0, ) -> dict: """Start leader/follower teleoperation on this gripper. @@ -2234,8 +2246,13 @@ def teleop_start( dq_max: Slave only — ceiling in rad/s on the leader velocity fed forward to the follower. ``0`` disables the feedforward. torque_limit_nm: Slave only — ceiling in Nm on the follower's own - torque; held over it the follower releases in place. ``0`` - disables the guard. See :class:`~litegrip.GripperTeleop`. + torque; held over it the follower acts per + ``torque_guard_action``. ``0`` disables the guard. See + :class:`~litegrip.GripperTeleop`. + torque_guard_action: Slave only — ``"hold"`` (default) keeps + pressing with a bounded pure-torque frame so a grasp survives; + ``"release"`` drops to zero stiffness and lets go, the previous + behaviour. lead_cap_mm: Slave only — ceiling in mm on how far the *align* command may lead the measured position, which bounds the align torque. The follow loop is not capped. ``0`` disables the cap. @@ -2249,6 +2266,17 @@ def teleop_start( ready_tolerance_mm: Slave only — arrival tolerance for announcing ready, in mm of travel. ready_period_s: Slave only — how often to republish the ready state. + force_feedback: Send the follower's squeeze back and render it on + the leader as resistance. Needs the zenoh link (the UDP master + is publish-only and silently runs without it). + feedback_gain_nm_per_n: Leader torque per newton of follower + squeeze above the threshold. ``0`` disables it. + feedback_threshold_n: Follower squeeze below which the leader feels + nothing. + feedback_max_tau_nm: Ceiling on the leader's rendered resistance. + feedback_damping: Leader damping applied only during contact. + feedback_timeout_s: Age past which the force is ignored and both + torque and damping drop to zero. rate_hz: Loop rate. Returns: @@ -2264,7 +2292,7 @@ def teleop_start( positive. """ from .teleop import (GripperTeleop, TeleopBusyError, check_ready, - ready_topic, teleop_topic) + feedback_topic, ready_topic, teleop_topic) from .teleop import UdpTeleopTransport self._check_connected() @@ -2286,6 +2314,11 @@ def teleop_start( try: key = teleop_topic(grip_id) ready_key = ready_topic(grip_id) + # Declared on every session, even with the feature off: the leader's + # zenoh endpoint is resident and its key set cannot change later. + # An unused key costs nothing and a later session that enables + # feedback then just works. + feedback_key = feedback_topic(grip_id) created_transport = None if transport is None: if link == "zenoh": @@ -2295,10 +2328,11 @@ def teleop_start( # 已死的端点 ⇒ 此后每一轮都在往死会话里 put。(真机实测: # 第一次配对正常,之后每次从端 0 帧。)常驻端点的生命周期 # 只归 `disconnect()` → `_close_teleop_pub()`。 - transport = self._open_teleop_pub(port, key, ready_key) + transport = self._open_teleop_pub(port, key, ready_key, + feedback_key) else: transport = _zenoh_transport("slave", key, port, host, - ready_key) + ready_key, feedback_key) created_transport = transport elif link == "udp": if host is None: @@ -2324,10 +2358,18 @@ def teleop_start( align_speed_mm_s=align_speed_mm_s, # [遥操对齐块] watchdog_s=watchdog_s, dq_max=dq_max, torque_limit_nm=torque_limit_nm, lead_cap_mm=lead_cap_mm, # [遥操对齐块] + torque_guard_action=torque_guard_action, ready_topic=ready_key, require_ready=require_ready, ready_timeout_s=ready_timeout_s, ready_tolerance_mm=ready_tolerance_mm, - ready_period_s=ready_period_s) + ready_period_s=ready_period_s, + force_feedback=force_feedback, + feedback_topic=feedback_key, + feedback_gain_nm_per_n=feedback_gain_nm_per_n, + feedback_threshold_n=feedback_threshold_n, + feedback_max_tau_nm=feedback_max_tau_nm, + feedback_damping=feedback_damping, + feedback_timeout_s=feedback_timeout_s) manager.start() except BaseException: self._release_session("teleop") @@ -2337,7 +2379,8 @@ def teleop_start( return manager.status() def _open_teleop_pub(self, port: int, key: str, - ready_key: Optional[str] = None) -> "TeleopTransport": + ready_key: Optional[str] = None, + feedback_key: Optional[str] = None) -> "TeleopTransport": """Return the leader's resident publisher, building it on first use. ⚠ **Resident, not per session.** Rebuilding the zenoh listener on every @@ -2349,7 +2392,7 @@ def _open_teleop_pub(self, port: int, key: str, """ if self._teleop_pub is None: self._teleop_pub = _zenoh_transport("master", key, port, None, - ready_key) + ready_key, feedback_key) return self._teleop_pub def _close_teleop_pub(self) -> None: diff --git a/src/litegrip/teleop.py b/src/litegrip/teleop.py index e6028f5..db00337 100644 --- a/src/litegrip/teleop.py +++ b/src/litegrip/teleop.py @@ -47,12 +47,21 @@ loop is deliberately left uncapped so the follower stays responsive; the ``torque_limit_nm`` guard is what protects it under load. * When ``torque_limit_nm`` is set (``0``, the default, disables it) the follower - watches its own torque and releases when it stays over the limit for - :data:`TORQUE_TRIP_CYCLES` cycles — see - :meth:`GripperTeleop._update_torque_guard`. Position is deliberately not used - for this: a jam partway through the travel is indistinguishable from slow + watches its own torque and, once it stays over the limit for + :data:`TORQUE_TRIP_CYCLES` cycles, holds there with a bounded pure-torque frame + (``torque_guard_action="hold"``, the default) — see + :meth:`GripperTeleop._update_torque_guard`. Holding rather than releasing is + what makes a grasp survive: zero stiffness lets whatever is between the jaws + push them straight back open. ``torque_guard_action="release"`` restores the + old release-in-place behaviour. Position is deliberately not used for the + decision: a jam partway through the travel is indistinguishable from slow motion, and the calibrated travel comes from two hand-measured limits, which makes it the least trustworthy number in the loop. +* ``force_feedback`` (``False``, the default) publishes the follower's own + squeeze on :func:`feedback_topic` each cycle; the master renders it as a + bounded resistance torque while its jaws are hand-movable, so the operator can + feel a grasp. It is coarse by design — a threshold, a proportional gain and a + ceiling. A transport that cannot subscribe (UDP master) simply runs without it. * A **non-finite** frame (NaN / ±inf) is *dropped*, never clamped. ``_clamp01`` passes NaN through and ``min(hi, NaN)`` returns ``hi``, so clamping a NaN target silently commands the follower to its closed stop. Bad readings are @@ -61,7 +70,7 @@ * 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. A - tripped torque guard overrides that and stays released. + tripped torque guard overrides that with its own bounded pure-torque hold. * The leader's readiness gate is about **when it hands back**, never about withholding frames: it publishes from the first cycle either way, so a follower always has something to align to and neither end can block on the @@ -185,6 +194,28 @@ #: after this. See :meth:`GripperTeleop._relaxed`. DEFAULT_READY_TIMEOUT_S = 10.0 +#: What the follower does when its torque guard trips. ``hold`` keeps pressing +#: with a bounded pure-torque frame (``kp=kd=0``, ``tau=±torque_limit_nm``) so a +#: grasp is not dropped; ``release`` sets zero stiffness and lets go, which is +#: what this module did before the option existed. The pure-torque hold mirrors +#: the force-hold path in :mod:`litegrip.actions`, which is validated on the +#: machine and keeps streaming to avoid the DM4310 comm-loss fault. +TORQUE_GUARD_HOLD = "hold" +TORQUE_GUARD_RELEASE = "release" +TORQUE_GUARD_ACTIONS = (TORQUE_GUARD_HOLD, TORQUE_GUARD_RELEASE) + +#: Force feedback defaults — coarse by design. ``gain`` converts newtons of the +#: follower's squeeze into newton-metres on the leader; ``threshold`` keeps light +#: contact and follow-loop friction silent; ``max_tau_nm`` is the safety ceiling +#: on what the leader's motor pushes into the operator's hand, and +#: ``timeout_s`` drops both the torque and the damping when the feedback channel +#: goes quiet so a dead link cannot pin a force on. ``0`` gain disables it. +DEFAULT_FEEDBACK_GAIN_NM_PER_N = 0.10 +DEFAULT_FEEDBACK_THRESHOLD_N = 2.0 +DEFAULT_FEEDBACK_MAX_TAU_NM = 1.0 +DEFAULT_FEEDBACK_DAMPING = 0.1 +DEFAULT_FEEDBACK_TIMEOUT_S = 0.3 + def encode_frame(openness: float, position_mm: float, force_n: float, timestamp: float) -> bytes: @@ -249,6 +280,51 @@ def decode_ready_frame(payload: bytes) -> bool: return _READY_FRAME.unpack(payload)[0] != 0 +# ── Feedback codec ──────────────────────────────────────────────────────── + +_FEEDBACK_FRAME = struct.Struct(">d") + +#: Size of one feedback frame in bytes (one big-endian double). +FEEDBACK_FRAME_SIZE = _FEEDBACK_FRAME.size + + +def encode_feedback_frame(force_n: float) -> bytes: + """Pack one follower-force frame — the squeeze, in newtons. + + Carried on its own topic rather than folded into the ready byte or the + teleop frame: the ready frame is pinned at one byte for older peers, and the + teleop frame is byte-identical to the litearm stack's. Reusing the *session* + is free; reusing the *frame* is not. ``force_n`` is the SDK's + mounting-normalised squeeze (``close_sign * torque_nm``), so positive always + means "closing on something" on either mount. + + Raises: + ValueError: ``force_n`` is not finite. A NaN would otherwise decode + into a leader target and be clamped onto a stop. + """ + value = float(force_n) + if not math.isfinite(value): + raise ValueError(f"feedback force must be finite, got {force_n!r}") + return _FEEDBACK_FRAME.pack(value) + + +def decode_feedback_frame(payload: bytes) -> float: + """Unpack a feedback frame into the follower squeeze in newtons. + + Raises: + ValueError: wrong length, or a non-finite value (the sender's clock is + not trusted, but its arithmetic is checked). + """ + if len(payload) != FEEDBACK_FRAME_SIZE: + raise ValueError( + f"feedback frame must be {FEEDBACK_FRAME_SIZE} bytes, " + f"got {len(payload)}") + value = _FEEDBACK_FRAME.unpack(payload)[0] + if not math.isfinite(value): + raise ValueError(f"feedback frame carries a non-finite force: {value!r}") + return value + + def teleop_topic(grip_id: str = DEFAULT_GRIP_ID) -> str: """Topic the leader publishes and the follower subscribes to. @@ -274,6 +350,17 @@ def ready_topic(grip_id: str = DEFAULT_GRIP_ID) -> str: return f"litearm/v4/{grip_id}/gripper_ready" +def feedback_topic(grip_id: str = DEFAULT_GRIP_ID) -> str: + """Topic the follower publishes its squeeze on and the leader subscribes to. + + A third sibling on the same transport, in the reverse direction. Like + :func:`ready_topic` it is an extension: a peer that does not know it never + matches, which is exactly the desired "old peer" behaviour — the handshake + still works and the force feedback is simply absent. + """ + return f"litearm/v4/{grip_id}/gripper_feedback" + + # ── Transport abstraction ───────────────────────────────────────────────── @@ -599,10 +686,13 @@ class GripperTeleop: ``kp * lead_cap_mm / rad_to_mm``. The follow loop is not capped. ``0`` disables the cap. See :meth:`_cap_lead`. torque_limit_nm: Slave only — ceiling in Nm on the follower's own torque. - Held over the limit for :data:`TORQUE_TRIP_CYCLES` cycles it - releases in place and stays released until the leader reopens by - :data:`TORQUE_REARM_OPENNESS`. ``0`` disables the guard. See - :meth:`_update_torque_guard`. + Held over the limit for :data:`TORQUE_TRIP_CYCLES` cycles it acts + according to ``torque_guard_action`` and stays in that state until + the leader reopens by :data:`TORQUE_REARM_OPENNESS`. ``0`` disables + the guard. See :meth:`_update_torque_guard`. + torque_guard_action: Slave only — ``"hold"`` (default) keeps pressing + with a bounded pure-torque frame so a grasp survives; ``"release"`` + drops to zero stiffness and lets go, the pre-existing behaviour. ready_topic: The follower-ready channel, or ``None`` to run without a handshake (the master then goes slack from its first cycle). When set, the master holds under gain until a ready frame arrives and the @@ -617,6 +707,26 @@ class GripperTeleop: position is within this many mm of the commanded one. ready_period_s: Slave only — republish the ready state at least this often (it is also resent on every change). + force_feedback: Publish the follower's squeeze and render it on the + leader as resistance. ``False`` (the default) keeps the old + behaviour. Needs ``feedback_topic`` and a subscribe-capable + transport on the master; otherwise it degrades to off. + feedback_topic: The follower-force channel. ``None`` disables force + feedback. + feedback_gain_nm_per_n: Leader torque per newton of follower squeeze + above the threshold. ``0`` disables it. + feedback_threshold_n: Follower squeeze below which the leader feels + nothing — keeps follow-loop friction and light contact silent. + feedback_max_tau_nm: Ceiling on the leader's rendered resistance, in Nm. + Keep it well under the motor limit: this drives the operator's + hand. + feedback_damping: Leader velocity damping (``kd``) applied only while a + fresh feedback frame exists. It is pure viscous braking (the master + sends ``kp=0`` and a zero velocity target) and breaks the + torque→open→torque-drop loop that couples the operator into the + follower. ``0`` disables it. + feedback_timeout_s: Age past which a feedback frame is ignored; both the + torque and the damping drop to zero. 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 @@ -637,12 +747,20 @@ def __init__( watchdog_s: float = 0.2, dq_max: float = DEFAULT_DQ_MAX, torque_limit_nm: float = DEFAULT_TORQUE_LIMIT_NM, + torque_guard_action: str = TORQUE_GUARD_HOLD, lead_cap_mm: float = DEFAULT_LEAD_CAP_MM, # [遥操对齐块] ready_topic: Optional[str] = None, require_ready: bool = True, ready_timeout_s: float = DEFAULT_READY_TIMEOUT_S, ready_tolerance_mm: float = DEFAULT_READY_TOLERANCE_MM, ready_period_s: float = DEFAULT_READY_PERIOD_S, + force_feedback: bool = False, + feedback_topic: Optional[str] = None, + feedback_gain_nm_per_n: float = DEFAULT_FEEDBACK_GAIN_NM_PER_N, + feedback_threshold_n: float = DEFAULT_FEEDBACK_THRESHOLD_N, + feedback_max_tau_nm: float = DEFAULT_FEEDBACK_MAX_TAU_NM, + feedback_damping: float = DEFAULT_FEEDBACK_DAMPING, + feedback_timeout_s: float = DEFAULT_FEEDBACK_TIMEOUT_S, sub_transport: Optional[TeleopTransport] = None, sleep_fn: Callable[[float], None] = time.sleep, time_fn: Callable[[], float] = time.monotonic, @@ -660,6 +778,29 @@ def __init__( raise ValueError( f"torque_limit_nm must be >= 0 (0 disables the guard), " f"got {torque_limit_nm!r}") + if torque_guard_action not in TORQUE_GUARD_ACTIONS: + raise ValueError( + f"torque_guard_action must be one of {TORQUE_GUARD_ACTIONS}, " + f"got {torque_guard_action!r}") + if float(feedback_gain_nm_per_n) < 0.0: + raise ValueError( + f"feedback_gain_nm_per_n must be >= 0 (0 disables it), " + f"got {feedback_gain_nm_per_n!r}") + if float(feedback_threshold_n) < 0.0: + raise ValueError( + f"feedback_threshold_n must be >= 0, " + f"got {feedback_threshold_n!r}") + if not 0.0 <= float(feedback_max_tau_nm) <= 10.0: + raise ValueError( + f"feedback_max_tau_nm must be in [0, 10], " + f"got {feedback_max_tau_nm!r}") + if not 0.0 <= float(feedback_damping) <= 5.0: + raise ValueError( + f"feedback_damping must be in [0, 5], " + f"got {feedback_damping!r}") + if not float(feedback_timeout_s) > 0.0: + raise ValueError( + f"feedback_timeout_s must be > 0, got {feedback_timeout_s!r}") if float(lead_cap_mm) < 0.0: raise ValueError( f"lead_cap_mm must be >= 0 (0 disables the cap), " @@ -685,12 +826,20 @@ def __init__( self._watchdog_s = watchdog_s self._dq_max = float(dq_max) self._torque_limit_nm = float(torque_limit_nm) + self._torque_guard_action = torque_guard_action self._lead_cap_mm = float(lead_cap_mm) self._ready_topic = ready_topic self._require_ready = bool(require_ready) self._ready_timeout_s = float(ready_timeout_s) self._ready_tolerance_mm = float(ready_tolerance_mm) self._ready_period_s = float(ready_period_s) + self._force_feedback = bool(force_feedback) + self._feedback_topic = feedback_topic + self._feedback_gain = float(feedback_gain_nm_per_n) + self._feedback_threshold_n = float(feedback_threshold_n) + self._feedback_max_tau_nm = float(feedback_max_tau_nm) + self._feedback_damping = float(feedback_damping) + self._feedback_timeout_s = float(feedback_timeout_s) self._sub_tp = sub_transport self._sleep_fn = sleep_fn self._time_fn = time_fn @@ -726,6 +875,19 @@ def __init__( self._torque_trips = 0 self._torque_over_cycles = 0 self._trip_openness = 0.0 + #: Sign latched at the trip, so the hold presses back into whatever + #: stopped the jaws rather than assuming the closing direction. + self._hold_tau_sign = 1.0 + #: Force feedback. Master side: ``_feedback_sub`` / ``_feedback_force_n`` + #: are the reverse channel and its newest sample; ``_feedback_rx_ts`` + #: gates it on :attr:`_feedback_timeout_s`. Slave side: ``_feedback_tx`` + #: counts what it published. + self._feedback_sub: Optional[TeleopSubscription] = None + self._feedback_force_n = 0.0 + self._feedback_rx_ts: Optional[float] = None + self._feedback_rx = 0 + self._feedback_tx = 0 + self._feedback_tau_nm = 0.0 #: Readiness handshake. Master side: ``_follower_ready`` is the last #: state heard, ``_master_relaxed`` latches once the gate opens so a #: later not-ready does not snatch control back from the operator, and @@ -812,8 +974,8 @@ def _handoff(self) -> None: """Leave the gripper holding position — never disable (§8 rule 4/6). A tripped torque guard is the exception: re-applying the follow gains - would press the very thing the guard just let go of, so the final frame - stays at zero torque. + would press the very thing the guard is holding, so the final frame is + the guard's own (bounded torque for ``hold``, zero for ``release``). """ try: if self._mode == "master": @@ -821,11 +983,11 @@ def _handoff(self) -> None: # gains, which is exactly the hand-off we want. self._g.exit_zero_gravity() else: - q = clamp_to_calibrated( - self._g.config, self._g.get_state(wait=False).position_rad) + state = self._g.get_state(wait=False) if self._over_torque: - self._send(q, 0.0, 0.0) + self._guard_hold_frame(state) else: + q = clamp_to_calibrated(self._g.config, state.position_rad) self._send(q, self._resolve_kp(), self._resolve_kd()) except Exception as e: # noqa: BLE001 log.debug("hand-off on stop failed: %s", e) @@ -858,7 +1020,8 @@ def status(self) -> dict: # The gripper's own error_code was not "enabled". "fault": self._fault, # Slave only: the follower's own torque (Nm), and whether the guard - # has released it. ``torque_trips`` counts trips this session. + # has tripped it (``hold`` keeps pressing, ``release`` lets go). + # ``torque_trips`` counts trips this session. "torque_nm": round(self._torque_nm, 4), "over_torque": self._over_torque, "torque_trips": self._torque_trips, @@ -874,12 +1037,20 @@ def status(self) -> dict: "ready_rx": self._ready_rx, "ready_pubs": self._ready_pubs, "ready_timed_out": self._ready_timed_out, + # Force feedback. ``feedback_force_n`` is the squeeze heard + # (master) or last published (slave); ``feedback_tau_nm`` is what + # the master is currently pushing into the operator's hand. + "feedback_force_n": round(self._feedback_force_n, 4), + "feedback_tau_nm": round(self._feedback_tau_nm, 4), + "feedback_rx": self._feedback_rx, + "feedback_tx": self._feedback_tx, } # ── master ──────────────────────────────────────────────────────── def _master_loop(self) -> None: self._ready_sub = self._open_ready_sub() + self._feedback_sub = self._open_feedback_sub() if self._ready_sub is None: log.info("[master] zero-gravity, publishing to %s", self._topic) else: @@ -888,6 +1059,15 @@ def _master_loop(self) -> None: log.info("[master] holding under gain, publishing to %s; releasing " "when %s reports ready (waiting %s)", self._topic, self._ready_topic, wait) + if self._feedback_sub is not None: + log.info("[master] force feedback on: %s (gain %.2f Nm/N, " + "threshold %.1f N, cap %.2f Nm)", + self._feedback_topic, self._feedback_gain, + self._feedback_threshold_n, self._feedback_max_tau_nm) + elif self._force_feedback: + log.warning("[master] force feedback requested but the transport " + "cannot provide %s; running without it", + self._feedback_topic) self._master_t0 = self._time_fn() try: while self._running: @@ -900,8 +1080,14 @@ def _master_loop(self) -> None: log.exception("[master] CAN error; loop exiting") break self._note_grip_fault(state) - if self._ready_sub is not None and not self._follower_ready: + # Both reverse channels are drained every cycle, not just until + # the gate opens: the ready state is latched once heard, but the + # force has to keep arriving for the whole session or the leader + # would stop feeling the grasp the moment it was handed back. + if self._ready_sub is not None: self._read_ready(self._ready_sub) + if self._feedback_sub is not None: + self._read_feedback(self._feedback_sub) # Keep the frame stream alive: the DM motor self-locks a # "communication loss" fault ~100 ms after frames stop, so a # frame goes out every cycle even though nothing is commanded. @@ -910,7 +1096,8 @@ def _master_loop(self) -> None: # which is the very thing the gate exists to prevent. try: if self._relaxed(): - self._send(0.0, 0.0, 0.0, quiet=True) + tau, damping = self._master_feedback() + self._send(0.0, 0.0, damping, tau=tau, quiet=True) else: self._send(clamp_to_calibrated( self._g.config, state.position_rad), @@ -1033,13 +1220,16 @@ def _slave_loop(self) -> None: # and runs at the loop rate. state = self._g.get_state(wait=False) self._note_grip_fault(state) + self._publish_feedback(state) self._update_torque_guard(state) self._announce_ready(state, cfg, q_cmd) if self._over_torque: - # Released in place: zero stiffness and damping, but the - # frames keep flowing so the motor does not latch its - # comm-loss fault. - self._send(q_cmd, 0.0, 0.0) + # Hold at the measured angle. ``hold`` keeps pressing + # with a bounded pure-torque frame so the grasp survives; + # ``release`` drops both gains. Either way the frames + # keep flowing so the motor does not latch its comm-loss + # fault. + self._guard_hold_frame(state) else: # Deliberately uncapped: the follower commands the # leader's opening outright so it stays responsive. Only @@ -1128,6 +1318,92 @@ def _read_ready(self, sub: TeleopSubscription) -> None: log.info("[master] follower %s", "ready" if ready else "not ready") self._follower_ready = ready + # ── force feedback ──────────────────────────────────────────────── + + def _open_feedback_sub(self) -> Optional[TeleopSubscription]: + """Subscribe to the follower-force channel, or ``None`` if it is off. + + Same rule as :meth:`_open_ready_sub`: a transport that cannot subscribe + (the UDP master) simply runs without feedback. This is why the feature + is safe to enable by default on a link that may be one-way. + """ + if not self._force_feedback or self._feedback_topic is None: + return None + try: + return (self._sub_tp or self._tp).sub(self._feedback_topic) + except Exception as e: # noqa: BLE001 + log.warning("[master] cannot subscribe to %s (%s); force feedback " + "is off", self._feedback_topic, e) + return None + + def _read_feedback(self, sub: TeleopSubscription) -> None: + """Drain the force channel. The newest sample wins.""" + msg = sub.drain_latest() + if msg is None: + return + try: + force = decode_feedback_frame(msg) + except ValueError as e: + log.debug("[master] ignoring bad feedback frame: %s", e) + return + self._feedback_force_n = force + self._feedback_rx_ts = self._time_fn() + self._feedback_rx += 1 + + def _master_feedback(self) -> Tuple[float, float]: + """The leader's ``(tau, kd)`` for this cycle. + + Coarse on purpose: only the follower's squeeze above + :attr:`_feedback_threshold_n` is reflected, proportional to + :attr:`_feedback_gain`, capped at :attr:`_feedback_max_tau_nm`, and + signed to *oppose closing* on this gripper's mount. The force on the + wire is already normalised so positive means "squeezing", and + ``close_sign`` is the leader's own closing direction — a wrong sign here + would drive the hand *into* the grasp, so it is covered by a test for + both mounts. + + Damping is returned only while there is real contact. With ``kp=0`` and + a zero velocity target the MIT law gives ``tau_motor = -kd*dq_measured``: + pure viscous braking, which breaks the follower-torque → leader-opens → + follower-opens → torque-drops loop without ever pushing the jaws closed. + Applied always, it would make a free-moving leader feel sticky. + + A stale or missing frame drops both to zero — a dead link must not pin a + force on the operator's hand. + """ + self._feedback_tau_nm = 0.0 + if not self._force_feedback or self._feedback_gain <= 0.0: + return 0.0, 0.0 + ts = self._feedback_rx_ts + if ts is None or (self._time_fn() - ts) > self._feedback_timeout_s: + return 0.0, 0.0 + squeeze = max(0.0, self._feedback_force_n - self._feedback_threshold_n) + mag = min(squeeze * self._feedback_gain, self._feedback_max_tau_nm) + if mag <= 0.0: + return 0.0, 0.0 + close_sign = float(getattr(self._g.config, "close_sign", 1.0) or 1.0) + tau = -close_sign * mag + self._feedback_tau_nm = tau + return tau, self._feedback_damping + + def _publish_feedback(self, state: Any) -> None: + """Publish this cycle's squeeze, slave side. Non-fatal on failure.""" + if not self._force_feedback or self._feedback_topic is None: + return + tp = self._ready_tp + if tp is None: + return + force = float(getattr(state, "force_n", 0.0) or 0.0) + if not math.isfinite(force): + return + self._feedback_force_n = force + try: + tp.pub(self._feedback_topic, encode_feedback_frame(force)) + self._feedback_tx += 1 + except Exception as e: # noqa: BLE001 + if self._feedback_tx == 0: + log.warning("[slave] feedback publish failed: %s", e) + def _relaxed(self) -> bool: """Whether the leader may hand back this cycle — **latched**. @@ -1301,11 +1577,11 @@ def _ramp_to(self, target_rad: float) -> None: self._note_grip_fault(state) self._update_torque_guard(state) if self._over_torque: - # Released in place, exactly as the follow loop would. Hold - # where the jaws are; do not keep driving into whatever - # tripped the guard. - self._send(clamp_to_calibrated(cfg, state.position_rad), - 0.0, 0.0) + # Exactly what the follow loop would do. Hold where the + # jaws are; do not keep driving into whatever tripped the + # guard (``hold`` leaves a bounded force on it, ``release`` + # lets go). + self._guard_hold_frame(state) return q_sched = start + dist_rad * (i / steps) self._send(self._cap_lead(q_sched, state.position_rad), @@ -1352,13 +1628,18 @@ def _count_rejected(self, got: Any) -> None: self._mode, got) def _send(self, q: float, kp: float, kd: float, dq: float = 0.0, - quiet: bool = False) -> None: + tau: float = 0.0, quiet: bool = False) -> None: """Send one MIT frame and **check the return value** (§8 rule 10). ``dq`` is the follower's velocity target: zero for a hold (and for the master's slack frames), the leader's finite-difference velocity for a follower cycle — see :meth:`_estimate_dq`. + ``tau`` is the feed-forward torque. It is how a bounded grasp is held + (with ``kp=kd=0`` it is the *only* torque) and how force feedback reaches + the operator's hand. The CAN layer rejects anything past the motor's + range rather than clamping it. + ``send_mit_frame`` returns ``False`` when the motor is not enabled or the CAN write fails — it does not raise, so ignoring the result means believing we are driving a gripper that is not moving. @@ -1369,7 +1650,7 @@ def _send(self, q: float, kp: float, kd: float, dq: float = 0.0, operator gets, and it should not make them check the 24 V supply when the interface went down. """ - if not self._g.send_mit_frame(q=q, kp=kp, kd=kd, dq=dq): + if not self._g.send_mit_frame(q=q, kp=kp, kd=kd, dq=dq, tau=tau): self._send_failed += 1 if self._send_failed == 1 and not quiet: error = getattr(self._g, "last_send_error", None) @@ -1393,7 +1674,7 @@ def _note_grip_fault(self, state: Any) -> None: log.warning("[%s] %s", self._mode, self._fault) def _update_torque_guard(self, state: Any) -> None: - """Release the follower when its torque stays over the limit. + """Hold or release the follower when its torque stays over the limit. Torque is the follower's own reading, not the leader's ``force_n`` on the wire: the jaws press whatever is between them, and the status frame's @@ -1402,10 +1683,17 @@ def _update_torque_guard(self, state: Any) -> None: motion, and the calibrated limits are the loop's least trustworthy numbers. - Latching matters: releasing in place drops the torque to zero, so a - non-latching guard would re-engage on the very next cycle and chatter. - The follower re-arms only once the leader has reopened by - :data:`TORQUE_REARM_OPENNESS`, i.e. once the operator has backed off. + Latching matters: either action leaves the follower in a state that no + longer tracks the leader, so a non-latching guard would re-engage on the + very next cycle and chatter. The follower re-arms only once the leader + has reopened by :data:`TORQUE_REARM_OPENNESS`, i.e. once the operator has + backed off. + + The hold direction is the sign of the torque at the trip, not + ``close_sign`` blindly: an obstruction hit while *opening* must be + pushed back along the opening direction, not the closing one. Latching + it also keeps the hold from flipping if the reading crosses zero once + the jaws settle. That target is capped at the open stop. A trip within the margin of full open has no travel left to reopen into, and an uncapped target @@ -1434,15 +1722,40 @@ def _update_torque_guard(self, state: Any) -> None: self._over_torque = True self._torque_trips += 1 self._trip_openness = self._last_openness - log.warning( - "[%s] torque %.3f Nm >= limit %.3f Nm for %d cycles — " - "releasing in place (reopen the leader to %.2f to re-arm)", - self._mode, self._torque_nm, self._torque_limit_nm, - TORQUE_TRIP_CYCLES, - min(self._last_openness + TORQUE_REARM_OPENNESS, 1.0)) + self._hold_tau_sign = 1.0 if self._torque_nm >= 0.0 else -1.0 + reopen = min(self._last_openness + TORQUE_REARM_OPENNESS, 1.0) + if self._torque_guard_action == TORQUE_GUARD_HOLD: + log.warning( + "[%s] torque %.3f Nm >= limit %.3f Nm for %d cycles — " + "holding at %.3f Nm (reopen the leader to %.2f to re-arm)", + self._mode, self._torque_nm, self._torque_limit_nm, + TORQUE_TRIP_CYCLES, + self._hold_tau_sign * self._torque_limit_nm, reopen) + else: + log.warning( + "[%s] torque %.3f Nm >= limit %.3f Nm for %d cycles — " + "releasing in place (reopen the leader to %.2f to re-arm)", + self._mode, self._torque_nm, self._torque_limit_nm, + TORQUE_TRIP_CYCLES, reopen) else: self._torque_over_cycles = 0 + def _guard_hold_frame(self, state: Any) -> None: + """One frame that keeps the follower where the guard put it. + + ``hold`` sends a pure-torque frame (``kp=kd=0``) at the latched limit, so + the jaws keep a bounded force on whatever stopped them — a grasp that + survives instead of a grasp that springs open. That is the same path + :mod:`litegrip.actions` uses for force hold. ``release`` sets zero + stiffness and lets the object push the jaws apart, as before. + """ + q = clamp_to_calibrated(self._g.config, state.position_rad) + if self._torque_guard_action == TORQUE_GUARD_HOLD: + self._send(q, 0.0, 0.0, + tau=self._hold_tau_sign * self._torque_limit_nm) + else: + self._send(q, 0.0, 0.0) + def _resolve_kp(self) -> float: """Follow gain: the explicit one, else the calibration's ``kp``.""" if self._kp is not None: diff --git a/src/litegrip/zenoh_link.py b/src/litegrip/zenoh_link.py index a96c32b..0a75b35 100644 --- a/src/litegrip/zenoh_link.py +++ b/src/litegrip/zenoh_link.py @@ -23,12 +23,13 @@ divergent formats would decode silently wrong, which is why the format is pinned in :mod:`litegrip.teleop`. -The readiness channel (``litearm/v4/{id}/gripper_ready``, see -:func:`~litegrip.teleop.ready_topic`) is a local extension: it is a second key on -the same session, in the reverse direction, and it is **not** part of the litearm -link — a litearm peer simply never declares it. It is only wired when a -``ready_key`` is passed, so an endpoint built without one is one-way, exactly as -before. +The reverse channels (``litearm/v4/{id}/gripper_ready`` and +``litearm/v4/{id}/gripper_feedback``, see +:func:`~litegrip.teleop.ready_topic` / :func:`~litegrip.teleop.feedback_topic`) +are local extensions: extra keys on the same session, in the reverse direction, +and **not** part of the litearm link — a litearm peer simply never declares them. +Each is only wired when its key is passed, so an endpoint built without one is +one-way, exactly as before. zenoh is an optional dependency: install it with ``pip install litegrip-python[zenoh]``. This module is imported lazily so the base SDK still works without it. @@ -92,15 +93,16 @@ def __exit__(self, *exc) -> bool: class Listener(_Endpoint): """Leader side: listen on a port and wait for a follower to connect. - ``ready_key`` is optional and adds the follower -> leader readiness channel: - a second declaration on the *same* session, so it needs no extra port. It - is an extension over ``liteteleop/link.py`` (which is one-way) and is only - declared when a key is given, so the one-way comparison still holds when it - is not. + ``ready_key`` and ``feedback_key`` are optional and add the follower -> + leader channels (readiness and squeeze): further declarations on the *same* + session, so they need no extra port. They are extensions over + ``liteteleop/link.py`` (which is one-way) and are only declared when a key is + given, so the one-way comparison still holds when they are not. """ def __init__(self, port: int, key: str, - ready_key: Optional[str] = None) -> None: + ready_key: Optional[str] = None, + feedback_key: Optional[str] = None) -> None: super().__init__() cfg = _base_config() cfg.insert_json5("listen/endpoints", f'["tcp/0.0.0.0:{int(port)}"]') @@ -125,6 +127,17 @@ def _on_ready(sample) -> None: self._ready_sub = self._session.declare_subscriber(ready_key, _on_ready) + self._feedback_slot: Optional[LatestSlot] = None + self._feedback_sub = None + if feedback_key: + self._feedback_slot = LatestSlot() + + def _on_feedback(sample) -> None: + self._feedback_slot.put(bytes(sample.payload), time.monotonic()) + + self._feedback_sub = self._session.declare_subscriber( + feedback_key, _on_feedback) + def put(self, payload: bytes) -> None: """Publish one frame. Non-blocking.""" self._pub.put(payload) @@ -133,6 +146,10 @@ def ready_subscription(self) -> "_SlotSubscription": """Subscribe-side handle for the readiness channel.""" return _SlotSubscription(self._ready_slot) + def feedback_subscription(self) -> "_SlotSubscription": + """Subscribe-side handle for the follower-force channel.""" + return _SlotSubscription(self._feedback_slot) + @property def matching(self) -> bool: """Whether any subscriber is matched. @@ -150,13 +167,14 @@ class Connector(_Endpoint): latest slot. Blocking work there (e.g. a ``goto_rad`` that waits for an ACK) stalls the zenoh thread. - ``ready_key`` is optional and adds the follower -> leader readiness channel - (a publisher on the same session); see :class:`Listener`. + ``ready_key`` / ``feedback_key`` are optional and add the follower -> leader + channels (publishers on the same session); see :class:`Listener`. """ def __init__(self, host: str, port: int, key: str, on_frame: Optional[Callable[[bytes], None]] = None, - ready_key: Optional[str] = None) -> None: + ready_key: Optional[str] = None, + feedback_key: Optional[str] = None) -> None: super().__init__() cfg = _base_config() cfg.insert_json5("listen/endpoints", "[]") @@ -176,12 +194,19 @@ def _handler(sample) -> None: self._sub = self._session.declare_subscriber(key, _handler) self._ready_pub = (self._session.declare_publisher(ready_key) if ready_key else None) + self._feedback_pub = (self._session.declare_publisher(feedback_key) + if feedback_key else None) def put_ready(self, payload: bytes) -> None: """Publish one ready frame. Non-blocking.""" if self._ready_pub is not None: self._ready_pub.put(payload) + def put_feedback(self, payload: bytes) -> None: + """Publish one follower-force frame. Non-blocking.""" + if self._feedback_pub is not None: + self._feedback_pub.put(payload) + def latest(self) -> int: """Frames received so far (used to display a rate).""" return self.received @@ -310,11 +335,15 @@ class ZenohTeleopTransport(TeleopTransport): When set, the *reverse* direction is carried on the same session: the master may ``sub`` it and the slave may ``pub`` it. ``None`` leaves the transport one-way, exactly as before. + feedback_key: The follower-force topic, normally + ``feedback_topic(grip_id)``. Same treatment as ``ready_key``, on + its own key so the pinned one-byte ready frame is untouched. """ def __init__(self, role: str, key: str, port: int = DEFAULT_GRIP_PORT, host: Optional[str] = None, - ready_key: Optional[str] = None) -> None: + ready_key: Optional[str] = None, + feedback_key: Optional[str] = None) -> None: if role not in ("master", "slave"): raise ValueError(f"role must be 'master' or 'slave', got {role!r}") if role == "slave" and not host: @@ -322,13 +351,16 @@ def __init__(self, role: str, key: str, port: int = DEFAULT_GRIP_PORT, self._role = role self._key = key self._ready_key = ready_key + self._feedback_key = feedback_key self._endpoint: Optional[_Endpoint] = None self._slot: Optional[LatestSlot] = None self._sub: Optional[_SlotSubscription] = None self._ready_sub: Optional[_SlotSubscription] = None + self._feedback_sub: Optional[_SlotSubscription] = None if role == "master": - self._endpoint = Listener(int(port), key, ready_key=ready_key) + self._endpoint = Listener(int(port), key, ready_key=ready_key, + feedback_key=feedback_key) else: self._slot = LatestSlot() @@ -339,7 +371,8 @@ def _on_wire(payload: bytes) -> None: self._slot.put(payload, time.monotonic()) self._endpoint = Connector(host, int(port), key, on_frame=_on_wire, - ready_key=ready_key) + ready_key=ready_key, + feedback_key=feedback_key) @property def matching(self) -> bool: @@ -356,12 +389,15 @@ def received(self) -> int: return 0 def pub(self, topic: str, payload: bytes) -> None: - # Each role publishes on exactly one key: the master its teleop frames, - # the slave its ready frames. + # Each role publishes on its own keys: the master its teleop frames, the + # slave its ready and follower-force frames. if self._role == "master": self._check_topic(topic, self._key) self._endpoint.put(payload) return + if self._feedback_key is not None and topic == self._feedback_key: + self._endpoint.put_feedback(payload) + return if self._ready_key is None: raise TeleopError( "this zenoh transport was built without a ready key; it cannot " @@ -371,12 +407,16 @@ def pub(self, topic: str, payload: bytes) -> None: def sub(self, topic: str) -> TeleopSubscription: # Mirror of ``pub``: the slave subscribes to the teleop key, the master - # to the ready key. + # to the ready and follower-force keys. if self._role == "slave": self._check_topic(topic, self._key) if self._sub is None: self._sub = _SlotSubscription(self._slot) return self._sub + if self._feedback_key is not None and topic == self._feedback_key: + if self._feedback_sub is None: + self._feedback_sub = self._endpoint.feedback_subscription() + return self._feedback_sub if self._ready_key is None: raise TeleopError( "this zenoh transport was built without a ready key; it cannot " diff --git a/tests/test_teleop.py b/tests/test_teleop.py index 761b259..fde755e 100644 --- a/tests/test_teleop.py +++ b/tests/test_teleop.py @@ -12,6 +12,7 @@ from __future__ import annotations +import struct import time import types import unittest @@ -19,21 +20,25 @@ import _sdkpath # noqa: F401 from litegrip import (DEFAULT_ALIGN_SPEED_MM_S, DEFAULT_DQ_MAX, - DEFAULT_LEAD_CAP_MM, DEFAULT_READY_TOLERANCE_MM, + DEFAULT_FEEDBACK_DAMPING, DEFAULT_LEAD_CAP_MM, + DEFAULT_READY_TOLERANCE_MM, FEEDBACK_FRAME_SIZE, FRAME_SIZE, READY_FRAME_SIZE, GripperTeleop, InProcTeleopTransport, TeleopBusyError, TeleopError, - TeleopNotReady, UdpTeleopTransport, decode_frame, - decode_ready_frame, encode_frame, encode_ready_frame, + TeleopNotReady, UdpTeleopTransport, decode_feedback_frame, + decode_frame, decode_ready_frame, encode_feedback_frame, + encode_frame, encode_ready_frame, feedback_topic, ready_topic, teleop_topic) from litegrip.teleop import (MAX_FRAME_GAP_S, TORQUE_REARM_OPENNESS, - TORQUE_TRIP_CYCLES, clamp_to_calibrated, check_ready, + TORQUE_TRIP_CYCLES, TORQUE_GUARD_RELEASE, + clamp_to_calibrated, check_ready, 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("gripA") READY = ready_topic("gripA") +FEEDBACK = feedback_topic("gripA") # 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. @@ -53,6 +58,24 @@ def _nop_sleep(_seconds: float) -> None: """Stub the loop's pacing so tests do not pay the frame interval.""" +def _publish_until(transport, topic, payload, predicate, + timeout_s: float = WAIT_S) -> bool: + """Republish ``payload`` until ``predicate()`` holds. + + The force channel has a timeout, and the loop under ``_nop_sleep`` runs far + faster than wall time, so a single frame is gone (stale) before a poll can + observe it. Keep feeding it, as a real follower would. + """ + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + transport.pub(topic, payload) + if predicate(): + return True + time.sleep(0.001) + transport.pub(topic, payload) + return predicate() + + class _PreSubTransport(InProcTeleopTransport): """In-process bus with the slave's subscription created up front.""" @@ -106,6 +129,35 @@ def test_ready_decode_rejects_wrong_size(self): with self.assertRaises(ValueError): decode_ready_frame(b"\x01\x00") + def test_feedback_topic_is_its_own_sibling(self): + # Its own key, so neither the pinned one-byte ready frame nor the + # byte-identical litearm teleop frame is touched. + self.assertEqual(FEEDBACK, "litearm/v4/gripA/gripper_feedback") + self.assertEqual(feedback_topic(), FEEDBACK) + self.assertNotIn(FEEDBACK, (teleop_topic(), ready_topic())) + + def test_feedback_frame_is_one_double(self): + self.assertEqual(FEEDBACK_FRAME_SIZE, 8) + self.assertEqual(len(encode_feedback_frame(0.0)), FEEDBACK_FRAME_SIZE) + + def test_feedback_roundtrip(self): + self.assertAlmostEqual( + decode_feedback_frame(encode_feedback_frame(7.25)), 7.25) + + def test_feedback_decode_rejects_wrong_size(self): + with self.assertRaises(ValueError): + decode_feedback_frame(b"") + with self.assertRaises(ValueError): + decode_feedback_frame(b"\x00" * (FEEDBACK_FRAME_SIZE - 1)) + + def test_feedback_codec_rejects_non_finite(self): + # A NaN would decode into a leader torque and be clamped onto a stop. + for bad in (float("nan"), float("inf"), float("-inf")): + with self.assertRaises(ValueError): + encode_feedback_frame(bad) + with self.assertRaises(ValueError): + decode_feedback_frame(struct.pack(">d", bad)) + class ConversionTest(unittest.TestCase): def test_travel_matches_limits(self): @@ -391,6 +443,32 @@ def test_start_wires_the_ready_topic(self): finally: g.teleop_stop() + def test_start_wires_the_feedback_topic_and_defaults_off(self): + g, _ = make_gripper() + transport = InProcTeleopTransport() + g.teleop_start("master", transport=transport) + try: + # Derived and wired even while off: the zenoh endpoint is resident + # and cannot learn a new key mid-session. + self.assertEqual(g._teleop._feedback_topic, FEEDBACK) + self.assertFalse(g._teleop._force_feedback) + finally: + g.teleop_stop() + + def test_start_passes_force_feedback_through(self): + g, _ = make_gripper() + transport = InProcTeleopTransport() + g.teleop_start("master", transport=transport, force_feedback=True, + feedback_gain_nm_per_n=0.2, + torque_guard_action=TORQUE_GUARD_RELEASE) + try: + self.assertTrue(g._teleop._force_feedback) + self.assertAlmostEqual(g._teleop._feedback_gain, 0.2) + self.assertEqual(g._teleop._torque_guard_action, + TORQUE_GUARD_RELEASE) + finally: + g.teleop_stop() + def test_start_can_opt_out_of_the_gate(self): g, _ = make_gripper() transport = InProcTeleopTransport() @@ -472,7 +550,7 @@ def pub(self, topic, payload): def close(self): self.closed = True - def _factory(role, key, port, host, ready_key=None): + def _factory(role, key, port, host, ready_key=None, feedback_key=None): built.append(_FakeLink()) return built[-1] @@ -851,13 +929,18 @@ def test_rejects_a_non_positive_period(self): class TorqueGuardTest(unittest.TestCase): - """The follower releases when its own torque stays over the limit. + """The follower holds when its own torque stays over the limit. Torque here is the follower's own reading — the status frame's ``tau``, derived from coil current — never the leader's ``force_n`` on the wire. Position is deliberately not consulted: a jam partway through the travel is indistinguishable from slow motion. The guard is opt-in (``0`` disables it), so the loop tests elsewhere run with it off. + + The default action is **hold**: a bounded pure-torque frame (``kp=kd=0``, + ``tau=±torque_limit_nm``) keeps pressing, so a grasp survives instead of + springing open. ``release`` restores the old zero-stiffness let-go and is + covered as the opt-in. """ def _mgr(self, **kwargs): @@ -960,65 +1043,295 @@ def test_rejects_a_negative_limit(self): GripperTeleop(g, InProcTeleopTransport(), "slave", TOPIC, torque_limit_nm=-0.1) - def test_slave_releases_in_place_and_keeps_streaming(self): - # A block partway through the travel, under a command to close: the - # follower presses it and its torque climbs past the limit. + def test_rejects_an_unknown_torque_guard_action(self): + g, _ = make_gripper() + with self.assertRaises(ValueError): + GripperTeleop(g, InProcTeleopTransport(), "slave", TOPIC, + torque_guard_action="explode") + + def test_rejects_a_negative_feedback_gain(self): + g, _ = make_gripper() + with self.assertRaises(ValueError): + GripperTeleop(g, InProcTeleopTransport(), "master", TOPIC, + feedback_gain_nm_per_n=-0.1) + + def test_rejects_a_feedback_cap_past_the_motor(self): + g, _ = make_gripper() + with self.assertRaises(ValueError): + GripperTeleop(g, InProcTeleopTransport(), "master", TOPIC, + feedback_max_tau_nm=11.0) + + def test_rejects_a_non_positive_feedback_timeout(self): + g, _ = make_gripper() + with self.assertRaises(ValueError): + GripperTeleop(g, InProcTeleopTransport(), "master", TOPIC, + feedback_timeout_s=0.0) + + # ── hold: the grasp survives ────────────────────────────────────── + + def _tripped_slave(self, **kwargs): + """A follower that closes onto a block until the guard trips.""" block = (POS_OPEN_RAD + POS_CLOSED_RAD) / 2.0 g, fake = make_gripper(start_rad=POS_OPEN_RAD, block_rad=block) transport = _PreSubTransport() mgr = GripperTeleop(g, transport, "slave", TOPIC, rate_hz=200.0, sleep_fn=_nop_sleep, align=False, watchdog_s=5.0, - torque_limit_nm=1.0) + torque_limit_nm=1.0, **kwargs) transport.pub(TOPIC, encode_frame(0.0, 0.0, 0.0, 0.0)) + return g, fake, mgr + + def test_slave_holds_with_bounded_torque_and_keeps_streaming(self): + # A block partway through the travel, under a command to close: the + # follower presses it and its torque climbs past the limit. The old + # behaviour dropped both gains, so the object pushed the jaws open — a + # grasp that springs. Now a pure-torque frame keeps a bounded squeeze. + g, fake, mgr = self._tripped_slave() mgr.start() try: self.assertTrue(_wait_until(lambda: mgr.status()["over_torque"])) - tripped = len(fake.frames) - # Released in place: zero stiffness, zero damping. + # Holding: zero stiffness, zero damping, bounded torque in the + # direction the jaws were pushing (here: closing, positive). self.assertTrue(_wait_until( lambda: fake.frames and fake.frames[-1].kp == 0.0 - and fake.frames[-1].kd == 0.0)) + and fake.frames[-1].kd == 0.0 + and abs(fake.frames[-1].tau_ff - 1.0) < 1e-9)) + tripped = len(fake.frames) # Still streaming, so the motor does not latch comm loss. self.assertTrue(_wait_until(lambda: len(fake.frames) > tripped + 5)) - self.assertEqual(fake.frames[-1].kp, 0.0) + self.assertAlmostEqual(fake.frames[-1].tau_ff, 1.0, places=6) + # Not released: the fake motor still reports the holding torque. + self.assertAlmostEqual(fake.motor.tau, 1.0, places=6) finally: mgr.stop() - def test_align_releases_in_place_when_the_guard_trips(self): - # The align used to run inside a blocking ``goto_rad``, outside the - # guard's reach: ``torque_limit_nm`` did not cover it, and a jam during - # the align pressed until the move finished. It now sends through the - # loop's own path, so the guard sees it. + def test_hold_pushes_along_the_trip_direction_on_a_reverse_mount(self): + # On the reverse mount closing decreases radians, so the torque at the + # trip is negative. The hold must follow *that* sign, not ``close_sign`` + # blindly — a wrong direction would press the jaws the other way. block = (POS_OPEN_RAD + POS_CLOSED_RAD) / 2.0 - g, fake = make_gripper(start_rad=POS_OPEN_RAD, block_rad=block) + g, fake = make_gripper(reverse=True, block_rad=block) transport = _PreSubTransport() mgr = GripperTeleop(g, transport, "slave", TOPIC, rate_hz=200.0, - sleep_fn=_nop_sleep, align=True, watchdog_s=5.0, + sleep_fn=_nop_sleep, align=False, watchdog_s=5.0, torque_limit_nm=1.0) transport.pub(TOPIC, encode_frame(0.0, 0.0, 0.0, 0.0)) mgr.start() try: + self.assertEqual(g.config.close_sign, -1.0) + self.assertTrue(_wait_until( + lambda: fake.frames and fake.frames[-1].kp == 0.0 + and abs(fake.frames[-1].tau_ff + 1.0) < 1e-9)) + finally: + mgr.stop() + + def test_release_action_is_the_opt_in_let_go(self): + g, fake, mgr = self._tripped_slave( + torque_guard_action=TORQUE_GUARD_RELEASE) + mgr.start() + try: + self.assertTrue(_wait_until(lambda: mgr.status()["over_torque"])) + # Released in place: zero stiffness, zero damping, no torque — the + # pre-existing behaviour, now only by request. self.assertTrue(_wait_until( - lambda: any(f.kp == 0.0 for f in fake.frames))) + lambda: fake.frames and fake.frames[-1].kp == 0.0 + and fake.frames[-1].kd == 0.0 + and fake.frames[-1].tau_ff == 0.0)) finally: mgr.stop() - def test_slave_releases_in_place_on_the_handoff(self): + def test_align_holds_when_the_guard_trips(self): + # The align used to run inside a blocking ``goto_rad``, outside the + # guard's reach: ``torque_limit_nm`` did not cover it, and a jam during + # the align pressed until the move finished. It now sends through the + # loop's own path, so the guard sees it — and holds like the follow loop. block = (POS_OPEN_RAD + POS_CLOSED_RAD) / 2.0 g, fake = make_gripper(start_rad=POS_OPEN_RAD, block_rad=block) transport = _PreSubTransport() mgr = GripperTeleop(g, transport, "slave", TOPIC, rate_hz=200.0, - sleep_fn=_nop_sleep, align=False, watchdog_s=5.0, + sleep_fn=_nop_sleep, align=True, watchdog_s=5.0, torque_limit_nm=1.0) transport.pub(TOPIC, encode_frame(0.0, 0.0, 0.0, 0.0)) mgr.start() + try: + self.assertTrue(_wait_until( + lambda: any(f.kp == 0.0 and abs(f.tau_ff - 1.0) < 1e-9 + for f in fake.frames))) + finally: + mgr.stop() + + def test_slave_holds_on_the_handoff(self): + g, fake, mgr = self._tripped_slave() + mgr.start() try: self.assertTrue(_wait_until(lambda: mgr.status()["over_torque"])) finally: mgr.stop() - # The final frame must not re-apply the gains and press again. + # The final frame must not re-apply the gains and press again — but it + # must keep the bounded hold, or the part drops the moment we stop. self.assertEqual(fake.frames[-1].kp, 0.0) self.assertEqual(fake.frames[-1].kd, 0.0) + self.assertAlmostEqual(fake.frames[-1].tau_ff, 1.0, places=6) + + +class ForceFeedbackTest(unittest.TestCase): + """The follower -> leader squeeze channel. + + Coarse by design: only the squeeze above the threshold is reflected, in + proportion to the gain, capped, and signed to oppose closing on the + leader's own mount. A wrong sign would drive the operator's hand *into* + the grasp, so both mounts matter. It degrades to off on any link that + cannot carry it (the publish-only UDP master, a stale channel). + """ + + @staticmethod + def _slave(**kwargs): + g, fake = make_gripper(start_rad=POS_CLOSED_RAD) + transport = _PreSubTransport() + fb_sub = transport.sub(FEEDBACK) # before start, so nothing is missed + mgr = GripperTeleop(g, transport, "slave", TOPIC, rate_hz=200.0, + sleep_fn=_nop_sleep, align=False, watchdog_s=5.0, + force_feedback=True, feedback_topic=FEEDBACK, + **kwargs) + return g, fake, transport, fb_sub, mgr + + def _master(self, **kwargs): + g, fake = make_gripper(start_rad=POS_OPEN_RAD) + # A real slack jaw does not move under zero gain; the fake one is purely + # kinematic, so freeze it or the slack frame would drag it closed. + fake.motor.step = lambda *a, **k: None + transport = InProcTeleopTransport() + mgr = GripperTeleop(g, transport, "master", TOPIC, rate_hz=200.0, + sleep_fn=_nop_sleep, force_feedback=True, + feedback_topic=FEEDBACK, **kwargs) + return g, fake, transport, mgr + + # ── follower ────────────────────────────────────────────────────── + + def test_slave_publishes_the_mount_normalised_squeeze(self): + g, fake, transport, fb_sub, mgr = self._slave() + # 0.5 Nm of coil torque on the shipped mount is +5 N of squeeze. + fake.get_torque = lambda: 0.5 + mgr.start() + try: + frame = [] + + def newest(): + payload = fb_sub.drain_latest() + if payload is not None: + frame[:] = [payload] + return bool(frame) + + self.assertTrue(_wait_until(newest)) + self.assertAlmostEqual(decode_feedback_frame(frame[0]), 5.0, + places=6) + self.assertGreater(mgr.status()["feedback_tx"], 0) + finally: + mgr.stop() + + # ── leader ──────────────────────────────────────────────────────── + + def _contact_frame(self, fake, g): + target = -g.config.close_sign * 0.8 + return [f for f in reversed(fake.frames) + if abs(f.tau_ff - target) < 1e-9] + + def test_master_renders_resistance_from_a_fresh_squeeze(self): + # 10 N of squeeze: (10 - 2) N * 0.10 Nm/N = 0.8 Nm, under the 1.0 cap, + # signed to oppose closing on the leader's mount. + g, fake, transport, mgr = self._master() + mgr.start() + try: + self.assertTrue(_wait_until(lambda: len(fake.frames) > 0)) + self.assertTrue(_publish_until( + transport, FEEDBACK, encode_feedback_frame(10.0), + lambda: bool(self._contact_frame(fake, g)))) + frame = self._contact_frame(fake, g)[0] + self.assertEqual((frame.kp, frame.kd), (0.0, DEFAULT_FEEDBACK_DAMPING)) + self.assertAlmostEqual(frame.tau_ff, -g.config.close_sign * 0.8, + places=6) + self.assertAlmostEqual(mgr.status()["feedback_tau_nm"], + -g.config.close_sign * 0.8, places=6) + finally: + mgr.stop() + + def test_master_feels_nothing_below_the_threshold(self): + g, fake, transport, mgr = self._master() + mgr.start() + try: + self.assertTrue(_publish_until( + transport, FEEDBACK, encode_feedback_frame(2.0), + lambda: mgr.status()["feedback_rx"] > 0)) + before = len(fake.frames) + self.assertTrue(_wait_until(lambda: len(fake.frames) > before + 5)) + self.assertEqual(fake.frames[-1].tau_ff, 0.0) + self.assertEqual(fake.frames[-1].kd, 0.0) + finally: + mgr.stop() + + def test_master_resistance_is_capped(self): + g, fake, transport, mgr = self._master(feedback_max_tau_nm=0.5) + mgr.start() + try: + self.assertTrue(_wait_until(lambda: len(fake.frames) > 0)) + target = -g.config.close_sign * 0.5 + self.assertTrue(_publish_until( + transport, FEEDBACK, encode_feedback_frame(10.0), + lambda: any(abs(f.tau_ff - target) < 1e-9 + for f in fake.frames))) + finally: + mgr.stop() + + def test_master_drops_the_force_when_the_channel_goes_quiet(self): + # A dead link must not pin a force on the operator's hand. + g, fake, transport, mgr = self._master(feedback_timeout_s=0.02) + mgr.start() + try: + self.assertTrue(_wait_until(lambda: len(fake.frames) > 0)) + self.assertTrue(_publish_until( + transport, FEEDBACK, encode_feedback_frame(10.0), + lambda: bool(self._contact_frame(fake, g)))) + # Stop feeding it: the loop's own clock runs past the timeout. + self.assertTrue(_wait_until( + lambda: fake.frames[-1].tau_ff == 0.0 + and fake.frames[-1].kd == 0.0)) + self.assertEqual(mgr.status()["feedback_tau_nm"], 0.0) + finally: + mgr.stop() + + def test_feedback_is_off_by_default(self): + g, fake = make_gripper(start_rad=POS_OPEN_RAD) + fake.motor.step = lambda *a, **k: None + transport = InProcTeleopTransport() + mgr = GripperTeleop(g, transport, "master", TOPIC, rate_hz=200.0, + sleep_fn=_nop_sleep, feedback_topic=FEEDBACK) + mgr.start() + try: + self.assertTrue(_wait_until(lambda: len(fake.frames) > 0)) + transport.pub(FEEDBACK, encode_feedback_frame(10.0)) + before = len(fake.frames) + self.assertTrue(_wait_until(lambda: len(fake.frames) > before + 5)) + self.assertEqual(fake.frames[-1].tau_ff, 0.0) + self.assertEqual(mgr.status()["feedback_rx"], 0) + finally: + mgr.stop() + + def test_a_publish_only_master_runs_without_feedback(self): + # The UDP master has no receive path at all; the feature must turn + # itself off rather than fail the session or hold a stale force. + g, fake = make_gripper(start_rad=POS_OPEN_RAD) + fake.motor.step = lambda *a, **k: None + transport = UdpTeleopTransport(pub_addr=("127.0.0.1", 1)) + mgr = GripperTeleop(g, transport, "master", TOPIC, rate_hz=200.0, + sleep_fn=_nop_sleep, force_feedback=True, + feedback_topic=FEEDBACK) + mgr.start() + try: + self.assertTrue(_wait_until(lambda: len(fake.frames) > 0)) + self.assertIsNone(mgr._feedback_sub) + self.assertEqual(fake.frames[-1].tau_ff, 0.0) + finally: + mgr.stop() + transport.close() class LeadCapTest(unittest.TestCase): From 2fb45d5203e1067ab5c2d12bd5b90bab15860fb2 Mon Sep 17 00:00:00 2001 From: yd-sl <166189879+yd-sl@users.noreply.github.com> Date: Thu, 12 Nov 2026 14:03:39 +0800 Subject: [PATCH 2/2] test: sample the leader feedback while the channel is still fed Reading status() after the publish loop exits is a race: the master loop keeps spinning with no further frames, so the 0.3 s timeout zeroes feedback_tau_nm before the assertion reads it. Take the status sample inside the predicate, while feedback is still being published. Seen on CI. --- tests/test_teleop.py | 25 +++++++++++++++++++------ 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/tests/test_teleop.py b/tests/test_teleop.py index fde755e..228d83c 100644 --- a/tests/test_teleop.py +++ b/tests/test_teleop.py @@ -1239,18 +1239,31 @@ def test_master_renders_resistance_from_a_fresh_squeeze(self): # 10 N of squeeze: (10 - 2) N * 0.10 Nm/N = 0.8 Nm, under the 1.0 cap, # signed to oppose closing on the leader's mount. g, fake, transport, mgr = self._master() + target = -g.config.close_sign * 0.8 mgr.start() try: self.assertTrue(_wait_until(lambda: len(fake.frames) > 0)) + # ⚠ The status sample has to be taken **while the channel is still + # being fed**. `sleep_fn` is a no-op, so the loop spins flat out + # and stops rendering `feedback_tau_nm` as soon as the last frame + # is older than the timeout — reading it after the publish loop + # exits is a race (it lost on a loaded CI runner). + seen = [] + + def rendered(): + st = mgr.status() + if (self._contact_frame(fake, g) + and abs(st["feedback_tau_nm"] - target) < 1e-6): + seen.append(st["feedback_tau_nm"]) + return True + return False + self.assertTrue(_publish_until( - transport, FEEDBACK, encode_feedback_frame(10.0), - lambda: bool(self._contact_frame(fake, g)))) + transport, FEEDBACK, encode_feedback_frame(10.0), rendered)) frame = self._contact_frame(fake, g)[0] self.assertEqual((frame.kp, frame.kd), (0.0, DEFAULT_FEEDBACK_DAMPING)) - self.assertAlmostEqual(frame.tau_ff, -g.config.close_sign * 0.8, - places=6) - self.assertAlmostEqual(mgr.status()["feedback_tau_nm"], - -g.config.close_sign * 0.8, places=6) + self.assertAlmostEqual(frame.tau_ff, target, places=6) + self.assertAlmostEqual(seen[0], target, places=6) finally: mgr.stop()