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..228d83c 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,308 @@ 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() + 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), rendered)) + frame = self._contact_frame(fake, g)[0] + self.assertEqual((frame.kp, frame.kd), (0.0, DEFAULT_FEEDBACK_DAMPING)) + self.assertAlmostEqual(frame.tau_ff, target, places=6) + self.assertAlmostEqual(seen[0], target, 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):