litegrip-python 1.2.1__py3-none-any.whl → 1.3.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
litegrip/__init__.py CHANGED
@@ -144,14 +144,25 @@ from .teleop import (
144
144
  DEFAULT_READY_TOLERANCE_MM,
145
145
  DEFAULT_READY_PERIOD_S,
146
146
  DEFAULT_READY_TIMEOUT_S,
147
+ DEFAULT_FEEDBACK_GAIN_NM_PER_N,
148
+ DEFAULT_FEEDBACK_THRESHOLD_N,
149
+ DEFAULT_FEEDBACK_MAX_TAU_NM,
150
+ DEFAULT_FEEDBACK_DAMPING,
151
+ DEFAULT_FEEDBACK_TIMEOUT_S,
152
+ TORQUE_GUARD_HOLD,
153
+ TORQUE_GUARD_RELEASE,
147
154
  FRAME_SIZE,
148
155
  READY_FRAME_SIZE,
156
+ FEEDBACK_FRAME_SIZE,
149
157
  encode_frame,
150
158
  decode_frame,
151
159
  encode_ready_frame,
152
160
  decode_ready_frame,
161
+ encode_feedback_frame,
162
+ decode_feedback_frame,
153
163
  teleop_topic,
154
164
  ready_topic,
165
+ feedback_topic,
155
166
  )
156
167
 
157
168
  # ── Trajectory record and replay ────────────────────────────────────────
@@ -268,14 +279,25 @@ __all__ = [
268
279
  "DEFAULT_READY_TOLERANCE_MM",
269
280
  "DEFAULT_READY_PERIOD_S",
270
281
  "DEFAULT_READY_TIMEOUT_S",
282
+ "DEFAULT_FEEDBACK_GAIN_NM_PER_N",
283
+ "DEFAULT_FEEDBACK_THRESHOLD_N",
284
+ "DEFAULT_FEEDBACK_MAX_TAU_NM",
285
+ "DEFAULT_FEEDBACK_DAMPING",
286
+ "DEFAULT_FEEDBACK_TIMEOUT_S",
287
+ "TORQUE_GUARD_HOLD",
288
+ "TORQUE_GUARD_RELEASE",
271
289
  "FRAME_SIZE",
272
290
  "READY_FRAME_SIZE",
291
+ "FEEDBACK_FRAME_SIZE",
273
292
  "encode_frame",
274
293
  "decode_frame",
275
294
  "encode_ready_frame",
276
295
  "decode_ready_frame",
296
+ "encode_feedback_frame",
297
+ "decode_feedback_frame",
277
298
  "teleop_topic",
278
299
  "ready_topic",
300
+ "feedback_topic",
279
301
  # Teleoperation — zenoh link (resolved lazily; needs litegrip-python[zenoh])
280
302
  "ZenohTeleopTransport",
281
303
  "Listener",
litegrip/gripper.py CHANGED
@@ -47,14 +47,19 @@ from .actions import (
47
47
  _check_calibrated,
48
48
  clamp_to_travel,
49
49
  )
50
- from .teleop import (DEFAULT_ALIGN_SPEED_MM_S, DEFAULT_DQ_MAX, DEFAULT_GRIP_ID,
50
+ from .teleop import (DEFAULT_ALIGN_SPEED_MM_S, DEFAULT_DQ_MAX,
51
+ DEFAULT_FEEDBACK_DAMPING, DEFAULT_FEEDBACK_GAIN_NM_PER_N,
52
+ DEFAULT_FEEDBACK_MAX_TAU_NM, DEFAULT_FEEDBACK_THRESHOLD_N,
53
+ DEFAULT_FEEDBACK_TIMEOUT_S, DEFAULT_GRIP_ID,
51
54
  DEFAULT_GRIP_PORT, DEFAULT_LEAD_CAP_MM,
52
55
  DEFAULT_READY_PERIOD_S, DEFAULT_READY_TIMEOUT_S,
53
- DEFAULT_READY_TOLERANCE_MM, DEFAULT_TORQUE_LIMIT_NM)
56
+ DEFAULT_READY_TOLERANCE_MM, DEFAULT_TORQUE_LIMIT_NM,
57
+ TORQUE_GUARD_HOLD)
54
58
 
55
59
 
56
60
  def _zenoh_transport(role: str, key: str, port: int, host: Optional[str],
57
- ready_key: Optional[str] = None) -> "TeleopTransport":
61
+ ready_key: Optional[str] = None,
62
+ feedback_key: Optional[str] = None) -> "TeleopTransport":
58
63
  """Build a point-to-point zenoh transport, or explain how to get one.
59
64
 
60
65
  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],
68
73
  "the zenoh teleoperation link needs the optional zenoh dependency — "
69
74
  "install it with `pip install litegrip-python[zenoh]`") from e
70
75
  return ZenohTeleopTransport(role, key, port=port, host=host,
71
- ready_key=ready_key)
76
+ ready_key=ready_key, feedback_key=feedback_key)
72
77
 
73
78
 
74
79
  # The calibration the SDK ships for the unit it ships with. This is the
@@ -2193,11 +2198,18 @@ class LiteGrip:
2193
2198
  watchdog_s: float = 0.2,
2194
2199
  dq_max: float = DEFAULT_DQ_MAX,
2195
2200
  torque_limit_nm: float = DEFAULT_TORQUE_LIMIT_NM,
2201
+ torque_guard_action: str = TORQUE_GUARD_HOLD,
2196
2202
  lead_cap_mm: float = DEFAULT_LEAD_CAP_MM, # [遥操对齐块]
2197
2203
  require_ready: bool = True,
2198
2204
  ready_timeout_s: float = DEFAULT_READY_TIMEOUT_S,
2199
2205
  ready_tolerance_mm: float = DEFAULT_READY_TOLERANCE_MM,
2200
2206
  ready_period_s: float = DEFAULT_READY_PERIOD_S,
2207
+ force_feedback: bool = False,
2208
+ feedback_gain_nm_per_n: float = DEFAULT_FEEDBACK_GAIN_NM_PER_N,
2209
+ feedback_threshold_n: float = DEFAULT_FEEDBACK_THRESHOLD_N,
2210
+ feedback_max_tau_nm: float = DEFAULT_FEEDBACK_MAX_TAU_NM,
2211
+ feedback_damping: float = DEFAULT_FEEDBACK_DAMPING,
2212
+ feedback_timeout_s: float = DEFAULT_FEEDBACK_TIMEOUT_S,
2201
2213
  rate_hz: float = 50.0,
2202
2214
  ) -> dict:
2203
2215
  """Start leader/follower teleoperation on this gripper.
@@ -2234,8 +2246,13 @@ class LiteGrip:
2234
2246
  dq_max: Slave only — ceiling in rad/s on the leader velocity fed
2235
2247
  forward to the follower. ``0`` disables the feedforward.
2236
2248
  torque_limit_nm: Slave only — ceiling in Nm on the follower's own
2237
- torque; held over it the follower releases in place. ``0``
2238
- disables the guard. See :class:`~litegrip.GripperTeleop`.
2249
+ torque; held over it the follower acts per
2250
+ ``torque_guard_action``. ``0`` disables the guard. See
2251
+ :class:`~litegrip.GripperTeleop`.
2252
+ torque_guard_action: Slave only — ``"hold"`` (default) keeps
2253
+ pressing with a bounded pure-torque frame so a grasp survives;
2254
+ ``"release"`` drops to zero stiffness and lets go, the previous
2255
+ behaviour.
2239
2256
  lead_cap_mm: Slave only — ceiling in mm on how far the *align*
2240
2257
  command may lead the measured position, which bounds the align
2241
2258
  torque. The follow loop is not capped. ``0`` disables the cap.
@@ -2249,6 +2266,17 @@ class LiteGrip:
2249
2266
  ready_tolerance_mm: Slave only — arrival tolerance for announcing
2250
2267
  ready, in mm of travel.
2251
2268
  ready_period_s: Slave only — how often to republish the ready state.
2269
+ force_feedback: Send the follower's squeeze back and render it on
2270
+ the leader as resistance. Needs the zenoh link (the UDP master
2271
+ is publish-only and silently runs without it).
2272
+ feedback_gain_nm_per_n: Leader torque per newton of follower
2273
+ squeeze above the threshold. ``0`` disables it.
2274
+ feedback_threshold_n: Follower squeeze below which the leader feels
2275
+ nothing.
2276
+ feedback_max_tau_nm: Ceiling on the leader's rendered resistance.
2277
+ feedback_damping: Leader damping applied only during contact.
2278
+ feedback_timeout_s: Age past which the force is ignored and both
2279
+ torque and damping drop to zero.
2252
2280
  rate_hz: Loop rate.
2253
2281
 
2254
2282
  Returns:
@@ -2264,7 +2292,7 @@ class LiteGrip:
2264
2292
  positive.
2265
2293
  """
2266
2294
  from .teleop import (GripperTeleop, TeleopBusyError, check_ready,
2267
- ready_topic, teleop_topic)
2295
+ feedback_topic, ready_topic, teleop_topic)
2268
2296
  from .teleop import UdpTeleopTransport
2269
2297
 
2270
2298
  self._check_connected()
@@ -2286,6 +2314,11 @@ class LiteGrip:
2286
2314
  try:
2287
2315
  key = teleop_topic(grip_id)
2288
2316
  ready_key = ready_topic(grip_id)
2317
+ # Declared on every session, even with the feature off: the leader's
2318
+ # zenoh endpoint is resident and its key set cannot change later.
2319
+ # An unused key costs nothing and a later session that enables
2320
+ # feedback then just works.
2321
+ feedback_key = feedback_topic(grip_id)
2289
2322
  created_transport = None
2290
2323
  if transport is None:
2291
2324
  if link == "zenoh":
@@ -2295,10 +2328,11 @@ class LiteGrip:
2295
2328
  # 已死的端点 ⇒ 此后每一轮都在往死会话里 put。(真机实测:
2296
2329
  # 第一次配对正常,之后每次从端 0 帧。)常驻端点的生命周期
2297
2330
  # 只归 `disconnect()` → `_close_teleop_pub()`。
2298
- transport = self._open_teleop_pub(port, key, ready_key)
2331
+ transport = self._open_teleop_pub(port, key, ready_key,
2332
+ feedback_key)
2299
2333
  else:
2300
2334
  transport = _zenoh_transport("slave", key, port, host,
2301
- ready_key)
2335
+ ready_key, feedback_key)
2302
2336
  created_transport = transport
2303
2337
  elif link == "udp":
2304
2338
  if host is None:
@@ -2324,10 +2358,18 @@ class LiteGrip:
2324
2358
  align_speed_mm_s=align_speed_mm_s, # [遥操对齐块]
2325
2359
  watchdog_s=watchdog_s, dq_max=dq_max,
2326
2360
  torque_limit_nm=torque_limit_nm, lead_cap_mm=lead_cap_mm, # [遥操对齐块]
2361
+ torque_guard_action=torque_guard_action,
2327
2362
  ready_topic=ready_key, require_ready=require_ready,
2328
2363
  ready_timeout_s=ready_timeout_s,
2329
2364
  ready_tolerance_mm=ready_tolerance_mm,
2330
- ready_period_s=ready_period_s)
2365
+ ready_period_s=ready_period_s,
2366
+ force_feedback=force_feedback,
2367
+ feedback_topic=feedback_key,
2368
+ feedback_gain_nm_per_n=feedback_gain_nm_per_n,
2369
+ feedback_threshold_n=feedback_threshold_n,
2370
+ feedback_max_tau_nm=feedback_max_tau_nm,
2371
+ feedback_damping=feedback_damping,
2372
+ feedback_timeout_s=feedback_timeout_s)
2331
2373
  manager.start()
2332
2374
  except BaseException:
2333
2375
  self._release_session("teleop")
@@ -2337,7 +2379,8 @@ class LiteGrip:
2337
2379
  return manager.status()
2338
2380
 
2339
2381
  def _open_teleop_pub(self, port: int, key: str,
2340
- ready_key: Optional[str] = None) -> "TeleopTransport":
2382
+ ready_key: Optional[str] = None,
2383
+ feedback_key: Optional[str] = None) -> "TeleopTransport":
2341
2384
  """Return the leader's resident publisher, building it on first use.
2342
2385
 
2343
2386
  ⚠ **Resident, not per session.** Rebuilding the zenoh listener on every
@@ -2349,7 +2392,7 @@ class LiteGrip:
2349
2392
  """
2350
2393
  if self._teleop_pub is None:
2351
2394
  self._teleop_pub = _zenoh_transport("master", key, port, None,
2352
- ready_key)
2395
+ ready_key, feedback_key)
2353
2396
  return self._teleop_pub
2354
2397
 
2355
2398
  def _close_teleop_pub(self) -> None:
litegrip/teleop.py CHANGED
@@ -47,12 +47,21 @@ Safety notes
47
47
  loop is deliberately left uncapped so the follower stays responsive; the
48
48
  ``torque_limit_nm`` guard is what protects it under load.
49
49
  * When ``torque_limit_nm`` is set (``0``, the default, disables it) the follower
50
- watches its own torque and releases when it stays over the limit for
51
- :data:`TORQUE_TRIP_CYCLES` cycles — see
52
- :meth:`GripperTeleop._update_torque_guard`. Position is deliberately not used
53
- for this: a jam partway through the travel is indistinguishable from slow
50
+ watches its own torque and, once it stays over the limit for
51
+ :data:`TORQUE_TRIP_CYCLES` cycles, holds there with a bounded pure-torque frame
52
+ (``torque_guard_action="hold"``, the default) — see
53
+ :meth:`GripperTeleop._update_torque_guard`. Holding rather than releasing is
54
+ what makes a grasp survive: zero stiffness lets whatever is between the jaws
55
+ push them straight back open. ``torque_guard_action="release"`` restores the
56
+ old release-in-place behaviour. Position is deliberately not used for the
57
+ decision: a jam partway through the travel is indistinguishable from slow
54
58
  motion, and the calibrated travel comes from two hand-measured limits, which
55
59
  makes it the least trustworthy number in the loop.
60
+ * ``force_feedback`` (``False``, the default) publishes the follower's own
61
+ squeeze on :func:`feedback_topic` each cycle; the master renders it as a
62
+ bounded resistance torque while its jaws are hand-movable, so the operator can
63
+ feel a grasp. It is coarse by design — a threshold, a proportional gain and a
64
+ ceiling. A transport that cannot subscribe (UDP master) simply runs without it.
56
65
  * A **non-finite** frame (NaN / ±inf) is *dropped*, never clamped. ``_clamp01``
57
66
  passes NaN through and ``min(hi, NaN)`` returns ``hi``, so clamping a NaN
58
67
  target silently commands the follower to its closed stop. Bad readings are
@@ -61,7 +70,7 @@ Safety notes
61
70
  * A ``slave`` whose leader goes quiet **holds** its last target at the follow
62
71
  gains (it does not relax to zero torque). The jaws therefore keep pressing
63
72
  whatever is between them — the same behaviour as the litearm original. A
64
- tripped torque guard overrides that and stays released.
73
+ tripped torque guard overrides that with its own bounded pure-torque hold.
65
74
  * The leader's readiness gate is about **when it hands back**, never about
66
75
  withholding frames: it publishes from the first cycle either way, so a
67
76
  follower always has something to align to and neither end can block on the
@@ -185,6 +194,28 @@ DEFAULT_READY_PERIOD_S = 0.1
185
194
  #: after this. See :meth:`GripperTeleop._relaxed`.
186
195
  DEFAULT_READY_TIMEOUT_S = 10.0
187
196
 
197
+ #: What the follower does when its torque guard trips. ``hold`` keeps pressing
198
+ #: with a bounded pure-torque frame (``kp=kd=0``, ``tau=±torque_limit_nm``) so a
199
+ #: grasp is not dropped; ``release`` sets zero stiffness and lets go, which is
200
+ #: what this module did before the option existed. The pure-torque hold mirrors
201
+ #: the force-hold path in :mod:`litegrip.actions`, which is validated on the
202
+ #: machine and keeps streaming to avoid the DM4310 comm-loss fault.
203
+ TORQUE_GUARD_HOLD = "hold"
204
+ TORQUE_GUARD_RELEASE = "release"
205
+ TORQUE_GUARD_ACTIONS = (TORQUE_GUARD_HOLD, TORQUE_GUARD_RELEASE)
206
+
207
+ #: Force feedback defaults — coarse by design. ``gain`` converts newtons of the
208
+ #: follower's squeeze into newton-metres on the leader; ``threshold`` keeps light
209
+ #: contact and follow-loop friction silent; ``max_tau_nm`` is the safety ceiling
210
+ #: on what the leader's motor pushes into the operator's hand, and
211
+ #: ``timeout_s`` drops both the torque and the damping when the feedback channel
212
+ #: goes quiet so a dead link cannot pin a force on. ``0`` gain disables it.
213
+ DEFAULT_FEEDBACK_GAIN_NM_PER_N = 0.10
214
+ DEFAULT_FEEDBACK_THRESHOLD_N = 2.0
215
+ DEFAULT_FEEDBACK_MAX_TAU_NM = 1.0
216
+ DEFAULT_FEEDBACK_DAMPING = 0.1
217
+ DEFAULT_FEEDBACK_TIMEOUT_S = 0.3
218
+
188
219
 
189
220
  def encode_frame(openness: float, position_mm: float, force_n: float,
190
221
  timestamp: float) -> bytes:
@@ -249,6 +280,51 @@ def decode_ready_frame(payload: bytes) -> bool:
249
280
  return _READY_FRAME.unpack(payload)[0] != 0
250
281
 
251
282
 
283
+ # ── Feedback codec ────────────────────────────────────────────────────────
284
+
285
+ _FEEDBACK_FRAME = struct.Struct(">d")
286
+
287
+ #: Size of one feedback frame in bytes (one big-endian double).
288
+ FEEDBACK_FRAME_SIZE = _FEEDBACK_FRAME.size
289
+
290
+
291
+ def encode_feedback_frame(force_n: float) -> bytes:
292
+ """Pack one follower-force frame — the squeeze, in newtons.
293
+
294
+ Carried on its own topic rather than folded into the ready byte or the
295
+ teleop frame: the ready frame is pinned at one byte for older peers, and the
296
+ teleop frame is byte-identical to the litearm stack's. Reusing the *session*
297
+ is free; reusing the *frame* is not. ``force_n`` is the SDK's
298
+ mounting-normalised squeeze (``close_sign * torque_nm``), so positive always
299
+ means "closing on something" on either mount.
300
+
301
+ Raises:
302
+ ValueError: ``force_n`` is not finite. A NaN would otherwise decode
303
+ into a leader target and be clamped onto a stop.
304
+ """
305
+ value = float(force_n)
306
+ if not math.isfinite(value):
307
+ raise ValueError(f"feedback force must be finite, got {force_n!r}")
308
+ return _FEEDBACK_FRAME.pack(value)
309
+
310
+
311
+ def decode_feedback_frame(payload: bytes) -> float:
312
+ """Unpack a feedback frame into the follower squeeze in newtons.
313
+
314
+ Raises:
315
+ ValueError: wrong length, or a non-finite value (the sender's clock is
316
+ not trusted, but its arithmetic is checked).
317
+ """
318
+ if len(payload) != FEEDBACK_FRAME_SIZE:
319
+ raise ValueError(
320
+ f"feedback frame must be {FEEDBACK_FRAME_SIZE} bytes, "
321
+ f"got {len(payload)}")
322
+ value = _FEEDBACK_FRAME.unpack(payload)[0]
323
+ if not math.isfinite(value):
324
+ raise ValueError(f"feedback frame carries a non-finite force: {value!r}")
325
+ return value
326
+
327
+
252
328
  def teleop_topic(grip_id: str = DEFAULT_GRIP_ID) -> str:
253
329
  """Topic the leader publishes and the follower subscribes to.
254
330
 
@@ -274,6 +350,17 @@ def ready_topic(grip_id: str = DEFAULT_GRIP_ID) -> str:
274
350
  return f"litearm/v4/{grip_id}/gripper_ready"
275
351
 
276
352
 
353
+ def feedback_topic(grip_id: str = DEFAULT_GRIP_ID) -> str:
354
+ """Topic the follower publishes its squeeze on and the leader subscribes to.
355
+
356
+ A third sibling on the same transport, in the reverse direction. Like
357
+ :func:`ready_topic` it is an extension: a peer that does not know it never
358
+ matches, which is exactly the desired "old peer" behaviour — the handshake
359
+ still works and the force feedback is simply absent.
360
+ """
361
+ return f"litearm/v4/{grip_id}/gripper_feedback"
362
+
363
+
277
364
  # ── Transport abstraction ─────────────────────────────────────────────────
278
365
 
279
366
 
@@ -599,10 +686,13 @@ class GripperTeleop:
599
686
  ``kp * lead_cap_mm / rad_to_mm``. The follow loop is not capped.
600
687
  ``0`` disables the cap. See :meth:`_cap_lead`.
601
688
  torque_limit_nm: Slave only — ceiling in Nm on the follower's own torque.
602
- Held over the limit for :data:`TORQUE_TRIP_CYCLES` cycles it
603
- releases in place and stays released until the leader reopens by
604
- :data:`TORQUE_REARM_OPENNESS`. ``0`` disables the guard. See
605
- :meth:`_update_torque_guard`.
689
+ Held over the limit for :data:`TORQUE_TRIP_CYCLES` cycles it acts
690
+ according to ``torque_guard_action`` and stays in that state until
691
+ the leader reopens by :data:`TORQUE_REARM_OPENNESS`. ``0`` disables
692
+ the guard. See :meth:`_update_torque_guard`.
693
+ torque_guard_action: Slave only — ``"hold"`` (default) keeps pressing
694
+ with a bounded pure-torque frame so a grasp survives; ``"release"``
695
+ drops to zero stiffness and lets go, the pre-existing behaviour.
606
696
  ready_topic: The follower-ready channel, or ``None`` to run without a
607
697
  handshake (the master then goes slack from its first cycle). When
608
698
  set, the master holds under gain until a ready frame arrives and the
@@ -617,6 +707,26 @@ class GripperTeleop:
617
707
  position is within this many mm of the commanded one.
618
708
  ready_period_s: Slave only — republish the ready state at least this
619
709
  often (it is also resent on every change).
710
+ force_feedback: Publish the follower's squeeze and render it on the
711
+ leader as resistance. ``False`` (the default) keeps the old
712
+ behaviour. Needs ``feedback_topic`` and a subscribe-capable
713
+ transport on the master; otherwise it degrades to off.
714
+ feedback_topic: The follower-force channel. ``None`` disables force
715
+ feedback.
716
+ feedback_gain_nm_per_n: Leader torque per newton of follower squeeze
717
+ above the threshold. ``0`` disables it.
718
+ feedback_threshold_n: Follower squeeze below which the leader feels
719
+ nothing — keeps follow-loop friction and light contact silent.
720
+ feedback_max_tau_nm: Ceiling on the leader's rendered resistance, in Nm.
721
+ Keep it well under the motor limit: this drives the operator's
722
+ hand.
723
+ feedback_damping: Leader velocity damping (``kd``) applied only while a
724
+ fresh feedback frame exists. It is pure viscous braking (the master
725
+ sends ``kp=0`` and a zero velocity target) and breaks the
726
+ torque→open→torque-drop loop that couples the operator into the
727
+ follower. ``0`` disables it.
728
+ feedback_timeout_s: Age past which a feedback frame is ignored; both the
729
+ torque and the damping drop to zero.
620
730
  sub_transport: Slave only — a separate transport to subscribe on when
621
731
  the leader is remote (the master's transport is local-only).
622
732
  sleep_fn, time_fn: Timing seams for tests. ``time_fn`` must be
@@ -637,12 +747,20 @@ class GripperTeleop:
637
747
  watchdog_s: float = 0.2,
638
748
  dq_max: float = DEFAULT_DQ_MAX,
639
749
  torque_limit_nm: float = DEFAULT_TORQUE_LIMIT_NM,
750
+ torque_guard_action: str = TORQUE_GUARD_HOLD,
640
751
  lead_cap_mm: float = DEFAULT_LEAD_CAP_MM, # [遥操对齐块]
641
752
  ready_topic: Optional[str] = None,
642
753
  require_ready: bool = True,
643
754
  ready_timeout_s: float = DEFAULT_READY_TIMEOUT_S,
644
755
  ready_tolerance_mm: float = DEFAULT_READY_TOLERANCE_MM,
645
756
  ready_period_s: float = DEFAULT_READY_PERIOD_S,
757
+ force_feedback: bool = False,
758
+ feedback_topic: Optional[str] = None,
759
+ feedback_gain_nm_per_n: float = DEFAULT_FEEDBACK_GAIN_NM_PER_N,
760
+ feedback_threshold_n: float = DEFAULT_FEEDBACK_THRESHOLD_N,
761
+ feedback_max_tau_nm: float = DEFAULT_FEEDBACK_MAX_TAU_NM,
762
+ feedback_damping: float = DEFAULT_FEEDBACK_DAMPING,
763
+ feedback_timeout_s: float = DEFAULT_FEEDBACK_TIMEOUT_S,
646
764
  sub_transport: Optional[TeleopTransport] = None,
647
765
  sleep_fn: Callable[[float], None] = time.sleep,
648
766
  time_fn: Callable[[], float] = time.monotonic,
@@ -660,6 +778,29 @@ class GripperTeleop:
660
778
  raise ValueError(
661
779
  f"torque_limit_nm must be >= 0 (0 disables the guard), "
662
780
  f"got {torque_limit_nm!r}")
781
+ if torque_guard_action not in TORQUE_GUARD_ACTIONS:
782
+ raise ValueError(
783
+ f"torque_guard_action must be one of {TORQUE_GUARD_ACTIONS}, "
784
+ f"got {torque_guard_action!r}")
785
+ if float(feedback_gain_nm_per_n) < 0.0:
786
+ raise ValueError(
787
+ f"feedback_gain_nm_per_n must be >= 0 (0 disables it), "
788
+ f"got {feedback_gain_nm_per_n!r}")
789
+ if float(feedback_threshold_n) < 0.0:
790
+ raise ValueError(
791
+ f"feedback_threshold_n must be >= 0, "
792
+ f"got {feedback_threshold_n!r}")
793
+ if not 0.0 <= float(feedback_max_tau_nm) <= 10.0:
794
+ raise ValueError(
795
+ f"feedback_max_tau_nm must be in [0, 10], "
796
+ f"got {feedback_max_tau_nm!r}")
797
+ if not 0.0 <= float(feedback_damping) <= 5.0:
798
+ raise ValueError(
799
+ f"feedback_damping must be in [0, 5], "
800
+ f"got {feedback_damping!r}")
801
+ if not float(feedback_timeout_s) > 0.0:
802
+ raise ValueError(
803
+ f"feedback_timeout_s must be > 0, got {feedback_timeout_s!r}")
663
804
  if float(lead_cap_mm) < 0.0:
664
805
  raise ValueError(
665
806
  f"lead_cap_mm must be >= 0 (0 disables the cap), "
@@ -685,12 +826,20 @@ class GripperTeleop:
685
826
  self._watchdog_s = watchdog_s
686
827
  self._dq_max = float(dq_max)
687
828
  self._torque_limit_nm = float(torque_limit_nm)
829
+ self._torque_guard_action = torque_guard_action
688
830
  self._lead_cap_mm = float(lead_cap_mm)
689
831
  self._ready_topic = ready_topic
690
832
  self._require_ready = bool(require_ready)
691
833
  self._ready_timeout_s = float(ready_timeout_s)
692
834
  self._ready_tolerance_mm = float(ready_tolerance_mm)
693
835
  self._ready_period_s = float(ready_period_s)
836
+ self._force_feedback = bool(force_feedback)
837
+ self._feedback_topic = feedback_topic
838
+ self._feedback_gain = float(feedback_gain_nm_per_n)
839
+ self._feedback_threshold_n = float(feedback_threshold_n)
840
+ self._feedback_max_tau_nm = float(feedback_max_tau_nm)
841
+ self._feedback_damping = float(feedback_damping)
842
+ self._feedback_timeout_s = float(feedback_timeout_s)
694
843
  self._sub_tp = sub_transport
695
844
  self._sleep_fn = sleep_fn
696
845
  self._time_fn = time_fn
@@ -726,6 +875,19 @@ class GripperTeleop:
726
875
  self._torque_trips = 0
727
876
  self._torque_over_cycles = 0
728
877
  self._trip_openness = 0.0
878
+ #: Sign latched at the trip, so the hold presses back into whatever
879
+ #: stopped the jaws rather than assuming the closing direction.
880
+ self._hold_tau_sign = 1.0
881
+ #: Force feedback. Master side: ``_feedback_sub`` / ``_feedback_force_n``
882
+ #: are the reverse channel and its newest sample; ``_feedback_rx_ts``
883
+ #: gates it on :attr:`_feedback_timeout_s`. Slave side: ``_feedback_tx``
884
+ #: counts what it published.
885
+ self._feedback_sub: Optional[TeleopSubscription] = None
886
+ self._feedback_force_n = 0.0
887
+ self._feedback_rx_ts: Optional[float] = None
888
+ self._feedback_rx = 0
889
+ self._feedback_tx = 0
890
+ self._feedback_tau_nm = 0.0
729
891
  #: Readiness handshake. Master side: ``_follower_ready`` is the last
730
892
  #: state heard, ``_master_relaxed`` latches once the gate opens so a
731
893
  #: later not-ready does not snatch control back from the operator, and
@@ -812,8 +974,8 @@ class GripperTeleop:
812
974
  """Leave the gripper holding position — never disable (§8 rule 4/6).
813
975
 
814
976
  A tripped torque guard is the exception: re-applying the follow gains
815
- would press the very thing the guard just let go of, so the final frame
816
- stays at zero torque.
977
+ would press the very thing the guard is holding, so the final frame is
978
+ the guard's own (bounded torque for ``hold``, zero for ``release``).
817
979
  """
818
980
  try:
819
981
  if self._mode == "master":
@@ -821,11 +983,11 @@ class GripperTeleop:
821
983
  # gains, which is exactly the hand-off we want.
822
984
  self._g.exit_zero_gravity()
823
985
  else:
824
- q = clamp_to_calibrated(
825
- self._g.config, self._g.get_state(wait=False).position_rad)
986
+ state = self._g.get_state(wait=False)
826
987
  if self._over_torque:
827
- self._send(q, 0.0, 0.0)
988
+ self._guard_hold_frame(state)
828
989
  else:
990
+ q = clamp_to_calibrated(self._g.config, state.position_rad)
829
991
  self._send(q, self._resolve_kp(), self._resolve_kd())
830
992
  except Exception as e: # noqa: BLE001
831
993
  log.debug("hand-off on stop failed: %s", e)
@@ -858,7 +1020,8 @@ class GripperTeleop:
858
1020
  # The gripper's own error_code was not "enabled".
859
1021
  "fault": self._fault,
860
1022
  # Slave only: the follower's own torque (Nm), and whether the guard
861
- # has released it. ``torque_trips`` counts trips this session.
1023
+ # has tripped it (``hold`` keeps pressing, ``release`` lets go).
1024
+ # ``torque_trips`` counts trips this session.
862
1025
  "torque_nm": round(self._torque_nm, 4),
863
1026
  "over_torque": self._over_torque,
864
1027
  "torque_trips": self._torque_trips,
@@ -874,12 +1037,20 @@ class GripperTeleop:
874
1037
  "ready_rx": self._ready_rx,
875
1038
  "ready_pubs": self._ready_pubs,
876
1039
  "ready_timed_out": self._ready_timed_out,
1040
+ # Force feedback. ``feedback_force_n`` is the squeeze heard
1041
+ # (master) or last published (slave); ``feedback_tau_nm`` is what
1042
+ # the master is currently pushing into the operator's hand.
1043
+ "feedback_force_n": round(self._feedback_force_n, 4),
1044
+ "feedback_tau_nm": round(self._feedback_tau_nm, 4),
1045
+ "feedback_rx": self._feedback_rx,
1046
+ "feedback_tx": self._feedback_tx,
877
1047
  }
878
1048
 
879
1049
  # ── master ────────────────────────────────────────────────────────
880
1050
 
881
1051
  def _master_loop(self) -> None:
882
1052
  self._ready_sub = self._open_ready_sub()
1053
+ self._feedback_sub = self._open_feedback_sub()
883
1054
  if self._ready_sub is None:
884
1055
  log.info("[master] zero-gravity, publishing to %s", self._topic)
885
1056
  else:
@@ -888,6 +1059,15 @@ class GripperTeleop:
888
1059
  log.info("[master] holding under gain, publishing to %s; releasing "
889
1060
  "when %s reports ready (waiting %s)",
890
1061
  self._topic, self._ready_topic, wait)
1062
+ if self._feedback_sub is not None:
1063
+ log.info("[master] force feedback on: %s (gain %.2f Nm/N, "
1064
+ "threshold %.1f N, cap %.2f Nm)",
1065
+ self._feedback_topic, self._feedback_gain,
1066
+ self._feedback_threshold_n, self._feedback_max_tau_nm)
1067
+ elif self._force_feedback:
1068
+ log.warning("[master] force feedback requested but the transport "
1069
+ "cannot provide %s; running without it",
1070
+ self._feedback_topic)
891
1071
  self._master_t0 = self._time_fn()
892
1072
  try:
893
1073
  while self._running:
@@ -900,8 +1080,14 @@ class GripperTeleop:
900
1080
  log.exception("[master] CAN error; loop exiting")
901
1081
  break
902
1082
  self._note_grip_fault(state)
903
- if self._ready_sub is not None and not self._follower_ready:
1083
+ # Both reverse channels are drained every cycle, not just until
1084
+ # the gate opens: the ready state is latched once heard, but the
1085
+ # force has to keep arriving for the whole session or the leader
1086
+ # would stop feeling the grasp the moment it was handed back.
1087
+ if self._ready_sub is not None:
904
1088
  self._read_ready(self._ready_sub)
1089
+ if self._feedback_sub is not None:
1090
+ self._read_feedback(self._feedback_sub)
905
1091
  # Keep the frame stream alive: the DM motor self-locks a
906
1092
  # "communication loss" fault ~100 ms after frames stop, so a
907
1093
  # frame goes out every cycle even though nothing is commanded.
@@ -910,7 +1096,8 @@ class GripperTeleop:
910
1096
  # which is the very thing the gate exists to prevent.
911
1097
  try:
912
1098
  if self._relaxed():
913
- self._send(0.0, 0.0, 0.0, quiet=True)
1099
+ tau, damping = self._master_feedback()
1100
+ self._send(0.0, 0.0, damping, tau=tau, quiet=True)
914
1101
  else:
915
1102
  self._send(clamp_to_calibrated(
916
1103
  self._g.config, state.position_rad),
@@ -1033,13 +1220,16 @@ class GripperTeleop:
1033
1220
  # and runs at the loop rate.
1034
1221
  state = self._g.get_state(wait=False)
1035
1222
  self._note_grip_fault(state)
1223
+ self._publish_feedback(state)
1036
1224
  self._update_torque_guard(state)
1037
1225
  self._announce_ready(state, cfg, q_cmd)
1038
1226
  if self._over_torque:
1039
- # Released in place: zero stiffness and damping, but the
1040
- # frames keep flowing so the motor does not latch its
1041
- # comm-loss fault.
1042
- self._send(q_cmd, 0.0, 0.0)
1227
+ # Hold at the measured angle. ``hold`` keeps pressing
1228
+ # with a bounded pure-torque frame so the grasp survives;
1229
+ # ``release`` drops both gains. Either way the frames
1230
+ # keep flowing so the motor does not latch its comm-loss
1231
+ # fault.
1232
+ self._guard_hold_frame(state)
1043
1233
  else:
1044
1234
  # Deliberately uncapped: the follower commands the
1045
1235
  # leader's opening outright so it stays responsive. Only
@@ -1128,6 +1318,92 @@ class GripperTeleop:
1128
1318
  log.info("[master] follower %s", "ready" if ready else "not ready")
1129
1319
  self._follower_ready = ready
1130
1320
 
1321
+ # ── force feedback ────────────────────────────────────────────────
1322
+
1323
+ def _open_feedback_sub(self) -> Optional[TeleopSubscription]:
1324
+ """Subscribe to the follower-force channel, or ``None`` if it is off.
1325
+
1326
+ Same rule as :meth:`_open_ready_sub`: a transport that cannot subscribe
1327
+ (the UDP master) simply runs without feedback. This is why the feature
1328
+ is safe to enable by default on a link that may be one-way.
1329
+ """
1330
+ if not self._force_feedback or self._feedback_topic is None:
1331
+ return None
1332
+ try:
1333
+ return (self._sub_tp or self._tp).sub(self._feedback_topic)
1334
+ except Exception as e: # noqa: BLE001
1335
+ log.warning("[master] cannot subscribe to %s (%s); force feedback "
1336
+ "is off", self._feedback_topic, e)
1337
+ return None
1338
+
1339
+ def _read_feedback(self, sub: TeleopSubscription) -> None:
1340
+ """Drain the force channel. The newest sample wins."""
1341
+ msg = sub.drain_latest()
1342
+ if msg is None:
1343
+ return
1344
+ try:
1345
+ force = decode_feedback_frame(msg)
1346
+ except ValueError as e:
1347
+ log.debug("[master] ignoring bad feedback frame: %s", e)
1348
+ return
1349
+ self._feedback_force_n = force
1350
+ self._feedback_rx_ts = self._time_fn()
1351
+ self._feedback_rx += 1
1352
+
1353
+ def _master_feedback(self) -> Tuple[float, float]:
1354
+ """The leader's ``(tau, kd)`` for this cycle.
1355
+
1356
+ Coarse on purpose: only the follower's squeeze above
1357
+ :attr:`_feedback_threshold_n` is reflected, proportional to
1358
+ :attr:`_feedback_gain`, capped at :attr:`_feedback_max_tau_nm`, and
1359
+ signed to *oppose closing* on this gripper's mount. The force on the
1360
+ wire is already normalised so positive means "squeezing", and
1361
+ ``close_sign`` is the leader's own closing direction — a wrong sign here
1362
+ would drive the hand *into* the grasp, so it is covered by a test for
1363
+ both mounts.
1364
+
1365
+ Damping is returned only while there is real contact. With ``kp=0`` and
1366
+ a zero velocity target the MIT law gives ``tau_motor = -kd*dq_measured``:
1367
+ pure viscous braking, which breaks the follower-torque → leader-opens →
1368
+ follower-opens → torque-drops loop without ever pushing the jaws closed.
1369
+ Applied always, it would make a free-moving leader feel sticky.
1370
+
1371
+ A stale or missing frame drops both to zero — a dead link must not pin a
1372
+ force on the operator's hand.
1373
+ """
1374
+ self._feedback_tau_nm = 0.0
1375
+ if not self._force_feedback or self._feedback_gain <= 0.0:
1376
+ return 0.0, 0.0
1377
+ ts = self._feedback_rx_ts
1378
+ if ts is None or (self._time_fn() - ts) > self._feedback_timeout_s:
1379
+ return 0.0, 0.0
1380
+ squeeze = max(0.0, self._feedback_force_n - self._feedback_threshold_n)
1381
+ mag = min(squeeze * self._feedback_gain, self._feedback_max_tau_nm)
1382
+ if mag <= 0.0:
1383
+ return 0.0, 0.0
1384
+ close_sign = float(getattr(self._g.config, "close_sign", 1.0) or 1.0)
1385
+ tau = -close_sign * mag
1386
+ self._feedback_tau_nm = tau
1387
+ return tau, self._feedback_damping
1388
+
1389
+ def _publish_feedback(self, state: Any) -> None:
1390
+ """Publish this cycle's squeeze, slave side. Non-fatal on failure."""
1391
+ if not self._force_feedback or self._feedback_topic is None:
1392
+ return
1393
+ tp = self._ready_tp
1394
+ if tp is None:
1395
+ return
1396
+ force = float(getattr(state, "force_n", 0.0) or 0.0)
1397
+ if not math.isfinite(force):
1398
+ return
1399
+ self._feedback_force_n = force
1400
+ try:
1401
+ tp.pub(self._feedback_topic, encode_feedback_frame(force))
1402
+ self._feedback_tx += 1
1403
+ except Exception as e: # noqa: BLE001
1404
+ if self._feedback_tx == 0:
1405
+ log.warning("[slave] feedback publish failed: %s", e)
1406
+
1131
1407
  def _relaxed(self) -> bool:
1132
1408
  """Whether the leader may hand back this cycle — **latched**.
1133
1409
 
@@ -1301,11 +1577,11 @@ class GripperTeleop:
1301
1577
  self._note_grip_fault(state)
1302
1578
  self._update_torque_guard(state)
1303
1579
  if self._over_torque:
1304
- # Released in place, exactly as the follow loop would. Hold
1305
- # where the jaws are; do not keep driving into whatever
1306
- # tripped the guard.
1307
- self._send(clamp_to_calibrated(cfg, state.position_rad),
1308
- 0.0, 0.0)
1580
+ # Exactly what the follow loop would do. Hold where the
1581
+ # jaws are; do not keep driving into whatever tripped the
1582
+ # guard (``hold`` leaves a bounded force on it, ``release``
1583
+ # lets go).
1584
+ self._guard_hold_frame(state)
1309
1585
  return
1310
1586
  q_sched = start + dist_rad * (i / steps)
1311
1587
  self._send(self._cap_lead(q_sched, state.position_rad),
@@ -1352,13 +1628,18 @@ class GripperTeleop:
1352
1628
  self._mode, got)
1353
1629
 
1354
1630
  def _send(self, q: float, kp: float, kd: float, dq: float = 0.0,
1355
- quiet: bool = False) -> None:
1631
+ tau: float = 0.0, quiet: bool = False) -> None:
1356
1632
  """Send one MIT frame and **check the return value** (§8 rule 10).
1357
1633
 
1358
1634
  ``dq`` is the follower's velocity target: zero for a hold (and for the
1359
1635
  master's slack frames), the leader's finite-difference velocity for a
1360
1636
  follower cycle — see :meth:`_estimate_dq`.
1361
1637
 
1638
+ ``tau`` is the feed-forward torque. It is how a bounded grasp is held
1639
+ (with ``kp=kd=0`` it is the *only* torque) and how force feedback reaches
1640
+ the operator's hand. The CAN layer rejects anything past the motor's
1641
+ range rather than clamping it.
1642
+
1362
1643
  ``send_mit_frame`` returns ``False`` when the motor is not enabled or the
1363
1644
  CAN write fails — it does not raise, so ignoring the result means
1364
1645
  believing we are driving a gripper that is not moving.
@@ -1369,7 +1650,7 @@ class GripperTeleop:
1369
1650
  operator gets, and it should not make them check the 24 V supply when the
1370
1651
  interface went down.
1371
1652
  """
1372
- if not self._g.send_mit_frame(q=q, kp=kp, kd=kd, dq=dq):
1653
+ if not self._g.send_mit_frame(q=q, kp=kp, kd=kd, dq=dq, tau=tau):
1373
1654
  self._send_failed += 1
1374
1655
  if self._send_failed == 1 and not quiet:
1375
1656
  error = getattr(self._g, "last_send_error", None)
@@ -1393,7 +1674,7 @@ class GripperTeleop:
1393
1674
  log.warning("[%s] %s", self._mode, self._fault)
1394
1675
 
1395
1676
  def _update_torque_guard(self, state: Any) -> None:
1396
- """Release the follower when its torque stays over the limit.
1677
+ """Hold or release the follower when its torque stays over the limit.
1397
1678
 
1398
1679
  Torque is the follower's own reading, not the leader's ``force_n`` on the
1399
1680
  wire: the jaws press whatever is between them, and the status frame's
@@ -1402,10 +1683,17 @@ class GripperTeleop:
1402
1683
  motion, and the calibrated limits are the loop's least trustworthy
1403
1684
  numbers.
1404
1685
 
1405
- Latching matters: releasing in place drops the torque to zero, so a
1406
- non-latching guard would re-engage on the very next cycle and chatter.
1407
- The follower re-arms only once the leader has reopened by
1408
- :data:`TORQUE_REARM_OPENNESS`, i.e. once the operator has backed off.
1686
+ Latching matters: either action leaves the follower in a state that no
1687
+ longer tracks the leader, so a non-latching guard would re-engage on the
1688
+ very next cycle and chatter. The follower re-arms only once the leader
1689
+ has reopened by :data:`TORQUE_REARM_OPENNESS`, i.e. once the operator has
1690
+ backed off.
1691
+
1692
+ The hold direction is the sign of the torque at the trip, not
1693
+ ``close_sign`` blindly: an obstruction hit while *opening* must be
1694
+ pushed back along the opening direction, not the closing one. Latching
1695
+ it also keeps the hold from flipping if the reading crosses zero once
1696
+ the jaws settle.
1409
1697
 
1410
1698
  That target is capped at the open stop. A trip within the margin of
1411
1699
  full open has no travel left to reopen into, and an uncapped target
@@ -1434,15 +1722,40 @@ class GripperTeleop:
1434
1722
  self._over_torque = True
1435
1723
  self._torque_trips += 1
1436
1724
  self._trip_openness = self._last_openness
1437
- log.warning(
1438
- "[%s] torque %.3f Nm >= limit %.3f Nm for %d cycles — "
1439
- "releasing in place (reopen the leader to %.2f to re-arm)",
1440
- self._mode, self._torque_nm, self._torque_limit_nm,
1441
- TORQUE_TRIP_CYCLES,
1442
- min(self._last_openness + TORQUE_REARM_OPENNESS, 1.0))
1725
+ self._hold_tau_sign = 1.0 if self._torque_nm >= 0.0 else -1.0
1726
+ reopen = min(self._last_openness + TORQUE_REARM_OPENNESS, 1.0)
1727
+ if self._torque_guard_action == TORQUE_GUARD_HOLD:
1728
+ log.warning(
1729
+ "[%s] torque %.3f Nm >= limit %.3f Nm for %d cycles — "
1730
+ "holding at %.3f Nm (reopen the leader to %.2f to re-arm)",
1731
+ self._mode, self._torque_nm, self._torque_limit_nm,
1732
+ TORQUE_TRIP_CYCLES,
1733
+ self._hold_tau_sign * self._torque_limit_nm, reopen)
1734
+ else:
1735
+ log.warning(
1736
+ "[%s] torque %.3f Nm >= limit %.3f Nm for %d cycles — "
1737
+ "releasing in place (reopen the leader to %.2f to re-arm)",
1738
+ self._mode, self._torque_nm, self._torque_limit_nm,
1739
+ TORQUE_TRIP_CYCLES, reopen)
1443
1740
  else:
1444
1741
  self._torque_over_cycles = 0
1445
1742
 
1743
+ def _guard_hold_frame(self, state: Any) -> None:
1744
+ """One frame that keeps the follower where the guard put it.
1745
+
1746
+ ``hold`` sends a pure-torque frame (``kp=kd=0``) at the latched limit, so
1747
+ the jaws keep a bounded force on whatever stopped them — a grasp that
1748
+ survives instead of a grasp that springs open. That is the same path
1749
+ :mod:`litegrip.actions` uses for force hold. ``release`` sets zero
1750
+ stiffness and lets the object push the jaws apart, as before.
1751
+ """
1752
+ q = clamp_to_calibrated(self._g.config, state.position_rad)
1753
+ if self._torque_guard_action == TORQUE_GUARD_HOLD:
1754
+ self._send(q, 0.0, 0.0,
1755
+ tau=self._hold_tau_sign * self._torque_limit_nm)
1756
+ else:
1757
+ self._send(q, 0.0, 0.0)
1758
+
1446
1759
  def _resolve_kp(self) -> float:
1447
1760
  """Follow gain: the explicit one, else the calibration's ``kp``."""
1448
1761
  if self._kp is not None:
litegrip/zenoh_link.py CHANGED
@@ -23,12 +23,13 @@ Two rules worth losing a session over:
23
23
  divergent formats would decode silently wrong, which is why the format is
24
24
  pinned in :mod:`litegrip.teleop`.
25
25
 
26
- The readiness channel (``litearm/v4/{id}/gripper_ready``, see
27
- :func:`~litegrip.teleop.ready_topic`) is a local extension: it is a second key on
28
- the same session, in the reverse direction, and it is **not** part of the litearm
29
- link — a litearm peer simply never declares it. It is only wired when a
30
- ``ready_key`` is passed, so an endpoint built without one is one-way, exactly as
31
- before.
26
+ The reverse channels (``litearm/v4/{id}/gripper_ready`` and
27
+ ``litearm/v4/{id}/gripper_feedback``, see
28
+ :func:`~litegrip.teleop.ready_topic` / :func:`~litegrip.teleop.feedback_topic`)
29
+ are local extensions: extra keys on the same session, in the reverse direction,
30
+ and **not** part of the litearm link — a litearm peer simply never declares them.
31
+ Each is only wired when its key is passed, so an endpoint built without one is
32
+ one-way, exactly as before.
32
33
 
33
34
  zenoh is an optional dependency: install it with ``pip install litegrip-python[zenoh]``.
34
35
  This module is imported lazily so the base SDK still works without it.
@@ -92,15 +93,16 @@ class _Endpoint:
92
93
  class Listener(_Endpoint):
93
94
  """Leader side: listen on a port and wait for a follower to connect.
94
95
 
95
- ``ready_key`` is optional and adds the follower -> leader readiness channel:
96
- a second declaration on the *same* session, so it needs no extra port. It
97
- is an extension over ``liteteleop/link.py`` (which is one-way) and is only
98
- declared when a key is given, so the one-way comparison still holds when it
99
- is not.
96
+ ``ready_key`` and ``feedback_key`` are optional and add the follower ->
97
+ leader channels (readiness and squeeze): further declarations on the *same*
98
+ session, so they need no extra port. They are extensions over
99
+ ``liteteleop/link.py`` (which is one-way) and are only declared when a key is
100
+ given, so the one-way comparison still holds when they are not.
100
101
  """
101
102
 
102
103
  def __init__(self, port: int, key: str,
103
- ready_key: Optional[str] = None) -> None:
104
+ ready_key: Optional[str] = None,
105
+ feedback_key: Optional[str] = None) -> None:
104
106
  super().__init__()
105
107
  cfg = _base_config()
106
108
  cfg.insert_json5("listen/endpoints", f'["tcp/0.0.0.0:{int(port)}"]')
@@ -125,6 +127,17 @@ class Listener(_Endpoint):
125
127
 
126
128
  self._ready_sub = self._session.declare_subscriber(ready_key, _on_ready)
127
129
 
130
+ self._feedback_slot: Optional[LatestSlot] = None
131
+ self._feedback_sub = None
132
+ if feedback_key:
133
+ self._feedback_slot = LatestSlot()
134
+
135
+ def _on_feedback(sample) -> None:
136
+ self._feedback_slot.put(bytes(sample.payload), time.monotonic())
137
+
138
+ self._feedback_sub = self._session.declare_subscriber(
139
+ feedback_key, _on_feedback)
140
+
128
141
  def put(self, payload: bytes) -> None:
129
142
  """Publish one frame. Non-blocking."""
130
143
  self._pub.put(payload)
@@ -133,6 +146,10 @@ class Listener(_Endpoint):
133
146
  """Subscribe-side handle for the readiness channel."""
134
147
  return _SlotSubscription(self._ready_slot)
135
148
 
149
+ def feedback_subscription(self) -> "_SlotSubscription":
150
+ """Subscribe-side handle for the follower-force channel."""
151
+ return _SlotSubscription(self._feedback_slot)
152
+
136
153
  @property
137
154
  def matching(self) -> bool:
138
155
  """Whether any subscriber is matched.
@@ -150,13 +167,14 @@ class Connector(_Endpoint):
150
167
  latest slot. Blocking work there (e.g. a ``goto_rad`` that waits for an ACK)
151
168
  stalls the zenoh thread.
152
169
 
153
- ``ready_key`` is optional and adds the follower -> leader readiness channel
154
- (a publisher on the same session); see :class:`Listener`.
170
+ ``ready_key`` / ``feedback_key`` are optional and add the follower -> leader
171
+ channels (publishers on the same session); see :class:`Listener`.
155
172
  """
156
173
 
157
174
  def __init__(self, host: str, port: int, key: str,
158
175
  on_frame: Optional[Callable[[bytes], None]] = None,
159
- ready_key: Optional[str] = None) -> None:
176
+ ready_key: Optional[str] = None,
177
+ feedback_key: Optional[str] = None) -> None:
160
178
  super().__init__()
161
179
  cfg = _base_config()
162
180
  cfg.insert_json5("listen/endpoints", "[]")
@@ -176,12 +194,19 @@ class Connector(_Endpoint):
176
194
  self._sub = self._session.declare_subscriber(key, _handler)
177
195
  self._ready_pub = (self._session.declare_publisher(ready_key)
178
196
  if ready_key else None)
197
+ self._feedback_pub = (self._session.declare_publisher(feedback_key)
198
+ if feedback_key else None)
179
199
 
180
200
  def put_ready(self, payload: bytes) -> None:
181
201
  """Publish one ready frame. Non-blocking."""
182
202
  if self._ready_pub is not None:
183
203
  self._ready_pub.put(payload)
184
204
 
205
+ def put_feedback(self, payload: bytes) -> None:
206
+ """Publish one follower-force frame. Non-blocking."""
207
+ if self._feedback_pub is not None:
208
+ self._feedback_pub.put(payload)
209
+
185
210
  def latest(self) -> int:
186
211
  """Frames received so far (used to display a rate)."""
187
212
  return self.received
@@ -310,11 +335,15 @@ class ZenohTeleopTransport(TeleopTransport):
310
335
  When set, the *reverse* direction is carried on the same session:
311
336
  the master may ``sub`` it and the slave may ``pub`` it. ``None``
312
337
  leaves the transport one-way, exactly as before.
338
+ feedback_key: The follower-force topic, normally
339
+ ``feedback_topic(grip_id)``. Same treatment as ``ready_key``, on
340
+ its own key so the pinned one-byte ready frame is untouched.
313
341
  """
314
342
 
315
343
  def __init__(self, role: str, key: str, port: int = DEFAULT_GRIP_PORT,
316
344
  host: Optional[str] = None,
317
- ready_key: Optional[str] = None) -> None:
345
+ ready_key: Optional[str] = None,
346
+ feedback_key: Optional[str] = None) -> None:
318
347
  if role not in ("master", "slave"):
319
348
  raise ValueError(f"role must be 'master' or 'slave', got {role!r}")
320
349
  if role == "slave" and not host:
@@ -322,13 +351,16 @@ class ZenohTeleopTransport(TeleopTransport):
322
351
  self._role = role
323
352
  self._key = key
324
353
  self._ready_key = ready_key
354
+ self._feedback_key = feedback_key
325
355
  self._endpoint: Optional[_Endpoint] = None
326
356
  self._slot: Optional[LatestSlot] = None
327
357
  self._sub: Optional[_SlotSubscription] = None
328
358
  self._ready_sub: Optional[_SlotSubscription] = None
359
+ self._feedback_sub: Optional[_SlotSubscription] = None
329
360
 
330
361
  if role == "master":
331
- self._endpoint = Listener(int(port), key, ready_key=ready_key)
362
+ self._endpoint = Listener(int(port), key, ready_key=ready_key,
363
+ feedback_key=feedback_key)
332
364
  else:
333
365
  self._slot = LatestSlot()
334
366
 
@@ -339,7 +371,8 @@ class ZenohTeleopTransport(TeleopTransport):
339
371
  self._slot.put(payload, time.monotonic())
340
372
 
341
373
  self._endpoint = Connector(host, int(port), key, on_frame=_on_wire,
342
- ready_key=ready_key)
374
+ ready_key=ready_key,
375
+ feedback_key=feedback_key)
343
376
 
344
377
  @property
345
378
  def matching(self) -> bool:
@@ -356,12 +389,15 @@ class ZenohTeleopTransport(TeleopTransport):
356
389
  return 0
357
390
 
358
391
  def pub(self, topic: str, payload: bytes) -> None:
359
- # Each role publishes on exactly one key: the master its teleop frames,
360
- # the slave its ready frames.
392
+ # Each role publishes on its own keys: the master its teleop frames, the
393
+ # slave its ready and follower-force frames.
361
394
  if self._role == "master":
362
395
  self._check_topic(topic, self._key)
363
396
  self._endpoint.put(payload)
364
397
  return
398
+ if self._feedback_key is not None and topic == self._feedback_key:
399
+ self._endpoint.put_feedback(payload)
400
+ return
365
401
  if self._ready_key is None:
366
402
  raise TeleopError(
367
403
  "this zenoh transport was built without a ready key; it cannot "
@@ -371,12 +407,16 @@ class ZenohTeleopTransport(TeleopTransport):
371
407
 
372
408
  def sub(self, topic: str) -> TeleopSubscription:
373
409
  # Mirror of ``pub``: the slave subscribes to the teleop key, the master
374
- # to the ready key.
410
+ # to the ready and follower-force keys.
375
411
  if self._role == "slave":
376
412
  self._check_topic(topic, self._key)
377
413
  if self._sub is None:
378
414
  self._sub = _SlotSubscription(self._slot)
379
415
  return self._sub
416
+ if self._feedback_key is not None and topic == self._feedback_key:
417
+ if self._feedback_sub is None:
418
+ self._feedback_sub = self._endpoint.feedback_subscription()
419
+ return self._feedback_sub
380
420
  if self._ready_key is None:
381
421
  raise TeleopError(
382
422
  "this zenoh transport was built without a ready key; it cannot "
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.4
2
2
  Name: litegrip-python
3
- Version: 1.2.1
3
+ Version: 1.3.0
4
4
  Summary: Python SDK for the LiteGrip lightweight robotic gripper series
5
5
  Author: NEXFORM ROBOTICS
6
6
  License-Expression: Apache-2.0
@@ -259,6 +259,14 @@ the closed-side stop) and stops there before the hold takes over.
259
259
  - **The jaws stay compliant.** An external push back-drives them while the motor keeps pushing
260
260
  with `force_n`, and an empty `grasp()` closes onto the mechanical stop at that force. That is
261
261
  the price of a force that does not drift when the workpiece moves.
262
+ - **Below about 2.2 N the jaws may not close at all**, so keep `force_n` at 3 N or more. The
263
+ closing leg's torque budget *is* the setpoint, and the mechanism needs about 0.2 Nm — 2.2 N of
264
+ setpoint — to break away from static friction. Under that the approach never moves, and
265
+ `grasp()` refuses the hand-over rather than reading the stillness as contact: `ok=False`,
266
+ `no_bite=True`, torque dropped, no hold frames. That refusal is the safe answer to a grip that
267
+ could not have closed; the alternative, handing over to a hold that presses at a slightly higher
268
+ torque, walks the jaws across the free stroke. Between 2.2 N and about 3 N it closes, but on this
269
+ bench the grip is marginal, so 3 N is the number to ask for.
262
270
  - **Below about 3.7 N the approach speed becomes the limit**, because one frame of travel at
263
271
  `grasp_speed_mm_s` already spends the whole torque budget. Cap the travel time with
264
272
  `grasp_speed_mm_s` rather than asking for a force the mechanism's own friction absorbs.
@@ -533,9 +541,9 @@ with LiteGrip("can0") as slave:
533
541
 
534
542
  | Method | What it does |
535
543
  | --- | --- |
536
- | `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. |
544
+ | `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. |
537
545
  | `teleop_stop(timeout=2.0)` → `dict` | Stops the loop and leaves the jaws holding. |
538
- | `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. |
546
+ | `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. |
539
547
 
540
548
  The link is a point-to-point **zenoh** session, an optional dependency:
541
549
 
@@ -565,11 +573,23 @@ python3 examples/teleop.py --mode slave --channel can0 --fake-leader --torque-li
565
573
  - **The leader holds its jaws under gain until the follower reports ready**, so the operator
566
574
  cannot push the target out from under a follower that is still travelling. `ready_timeout_s`
567
575
  (default `10.0`) bounds the wait; `require_ready=False` restores the immediate hand-back.
568
- - **An optional torque guard releases a follower that is pressing too hard.** Set
569
- `torque_limit_nm` and it goes to zero stiffness and damping *in place* after 60 ms over the
570
- limit, then re-arms once the leader has reopened. The default is `0` — **the guard is off
571
- unless you ask for it**. Pick the value per machine: it depends on how fragile the part between
572
- the jaws is.
576
+ - **An optional torque guard stops a follower that is pressing too hard.** Set
577
+ `torque_limit_nm` and, after 60 ms over the limit, the follower stops following and **keeps
578
+ pressing with that torque** (`kp=kd=0`, `tau=±torque_limit_nm`, signed by the torque measured
579
+ at the trip) so a clamped part is not pushed open; it re-arms once the leader has reopened.
580
+ `torque_guard_action="release"` restores the previous behaviour — zero stiffness and damping
581
+ *in place* — which lets the part push the jaws straight open. The default is `0` — **the guard
582
+ is off unless you ask for it**. Pick the value per machine: it depends on how fragile the part
583
+ between the jaws is.
584
+ - **The follower's squeeze can be felt on the leader.** With `force_feedback=True` the follower
585
+ publishes `force_n` on `litearm/v4/{grip_id}/gripper_feedback` (positive means closing,
586
+ normalised for the mount) and the leader renders resistance at
587
+ `min(max(0, force_n - feedback_threshold_n) * feedback_gain_nm_per_n, feedback_max_tau_nm)`,
588
+ directed against closing. Defaults: threshold `2.0` N, gain `0.10` Nm/N, cap `1.0` Nm.
589
+ Damping (`0.1`) applies only while the force is fresh; torque and damping both drop to zero
590
+ after `feedback_timeout_s` (`0.3`). It is off by default and needs the zenoh link — a UDP
591
+ leader is publish-only, so it runs without feedback. The key is additive, so an older peer
592
+ simply runs position-only teleoperation.
573
593
  - **A follower that loses the leader holds its position, it does not go slack.** After
574
594
  `watchdog_s` (default `0.2`) without a fresh frame, `stale` goes true and the jaws stay put.
575
595
  - **The follower feeds the leader's velocity forward** by differencing successive frames, so it
@@ -1,16 +1,16 @@
1
- litegrip/__init__.py,sha256=1emjAg7k1jLYsmFWVAg7r6jfJV6eZViYDnyRMcofCNM,9386
1
+ litegrip/__init__.py,sha256=ZaHmbsCABwp9mv4slhZ8EDKQ7skV11aPPsf5b_z-cKE,10034
2
2
  litegrip/actions.py,sha256=hYWjI_ww9KENPpGQbW72x5CTNVg7lisgscXaF3ZbK6M,110188
3
3
  litegrip/calibration_normal.json,sha256=rL9XGWD-aATZWUqE3MSXuQ2wR_Oz1L0EF78kJgQpnd0,209
4
4
  litegrip/calibration_reverse.json,sha256=HVweCi6LVBy7QQGxHa5NeaJfoOAfnzPMmVtXXIiCE74,209
5
5
  litegrip/constants.py,sha256=A1-gt-bEk4xsX00LJtE-0uQQu-Brz8LRIJOAVQfBpfg,6660
6
6
  litegrip/exceptions.py,sha256=2fQtANhxat0lVbhhU_SD-6nTcA4r-NLCpfmd06TGe3U,1144
7
7
  litegrip/factory_calibration.json,sha256=j4kbECLTXRPyfrApraDm2PlNRa1w7mnuhO6sQVQXSxU,345
8
- litegrip/gripper.py,sha256=9bKLrE0Xk2jFj7q4vTTo8-KQ1XZje8vcHTqgIRQhidg,124188
8
+ litegrip/gripper.py,sha256=Sy4kYZ25dH-PMdFQW7_QiTpxeO18Wi1leP1veK_bKxg,126912
9
9
  litegrip/models.py,sha256=oS36QK78uiDQFEydupfHMjIk02je75SFC6hvfW2abtM,7669
10
10
  litegrip/py.typed,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
11
- litegrip/teleop.py,sha256=1a0MUvpjMaHhjcTO9_T8OHzG-r-zlHGuk54OTMUJAbg,71719
11
+ litegrip/teleop.py,sha256=XyaG6DgFcMq6j1ABq0VlHPXAegOUuUEuKyZMPG21Sco,88372
12
12
  litegrip/trajectory.py,sha256=n1m97jADWvyk6FdUQGA69g8D2mFwD0bg0o5Zh9Mrpgg,44081
13
- litegrip/zenoh_link.py,sha256=t9NzlDhG0vJmUuw6B0B7WkXSNgxDH5EhiKIJAmuHDaY,16264
13
+ litegrip/zenoh_link.py,sha256=DvGI-z7gdovAkMPXIhogb0PvWTgHw5PQDJLRv_T_ML0,18315
14
14
  litegrip/can/__init__.py,sha256=MmK9W41RVx8tmwBMUbezlBbeMUC34pgsw6LveKQ8FKg,1019
15
15
  litegrip/can/controller.py,sha256=E8kPcKn0vHOVyxGhrZiiECEKFoY5tUEBvr6-_rw3hMM,16343
16
16
  litegrip/can/motor.py,sha256=rdcYK3rA3-yu4vWHA3sVM_DZY-rEeSGXGM5mLtff7AI,5753
@@ -18,8 +18,8 @@ litegrip/can/protocol.py,sha256=dt69WYOtVGSlmNv_Vxszt4YLkazUvl4dWs4S75wgZqg,1195
18
18
  litegrip/can/transport.py,sha256=4-0XjPpguEaFVMVHsD5iLBTjgKN8Cb-X1MYTZJkvhPE,14188
19
19
  litegrip/protocols/__init__.py,sha256=aregvFK6W76DGGrKCS1tccWsqWWCFBROXZPQ9CcPI1k,304
20
20
  litegrip/protocols/can_bus.py,sha256=UhGhVMrgQ--hC-hZNvPscrT2sjdMX3rgspOdCeKXAnY,26383
21
- litegrip_python-1.2.1.dist-info/licenses/LICENSE,sha256=yVuuHRzgI17MzTVgt3LsHvuX80innw--CmNPDCzO_iw,11358
22
- litegrip_python-1.2.1.dist-info/METADATA,sha256=u1QzJS1JSDBwjSbCgvKQm4UO5s_QXi7bprnn1iP5ZYg,45800
23
- litegrip_python-1.2.1.dist-info/WHEEL,sha256=mOdXR9C3wMVov7cSY8PY-XCzo3SmmWCHvGQr5csJoao,91
24
- litegrip_python-1.2.1.dist-info/top_level.txt,sha256=P4onDW9LkBEwiiQw4u1ui2fZSjQ4_XL5RZeCUh7RGoE,9
25
- litegrip_python-1.2.1.dist-info/RECORD,,
21
+ litegrip_python-1.3.0.dist-info/licenses/LICENSE,sha256=yVuuHRzgI17MzTVgt3LsHvuX80innw--CmNPDCzO_iw,11358
22
+ litegrip_python-1.3.0.dist-info/METADATA,sha256=fMeXTCPPv7IIf293a7nVI-MJG1l1NagT6rcXhl6mVDI,47741
23
+ litegrip_python-1.3.0.dist-info/WHEEL,sha256=mOdXR9C3wMVov7cSY8PY-XCzo3SmmWCHvGQr5csJoao,91
24
+ litegrip_python-1.3.0.dist-info/top_level.txt,sha256=P4onDW9LkBEwiiQw4u1ui2fZSjQ4_XL5RZeCUh7RGoE,9
25
+ litegrip_python-1.3.0.dist-info/RECORD,,