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 +22 -0
- litegrip/gripper.py +55 -12
- litegrip/teleop.py +352 -39
- litegrip/zenoh_link.py +61 -21
- {litegrip_python-1.2.1.dist-info → litegrip_python-1.3.0.dist-info}/METADATA +28 -8
- {litegrip_python-1.2.1.dist-info → litegrip_python-1.3.0.dist-info}/RECORD +9 -9
- {litegrip_python-1.2.1.dist-info → litegrip_python-1.3.0.dist-info}/WHEEL +0 -0
- {litegrip_python-1.2.1.dist-info → litegrip_python-1.3.0.dist-info}/licenses/LICENSE +0 -0
- {litegrip_python-1.2.1.dist-info → litegrip_python-1.3.0.dist-info}/top_level.txt +0 -0
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,
|
|
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
|
|
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
|
|
2238
|
-
disables the guard. See
|
|
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
|
|
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
|
|
51
|
-
:data:`TORQUE_TRIP_CYCLES` cycles
|
|
52
|
-
|
|
53
|
-
|
|
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
|
|
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
|
-
|
|
604
|
-
:data:`TORQUE_REARM_OPENNESS`. ``0`` disables
|
|
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
|
|
816
|
-
|
|
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
|
-
|
|
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.
|
|
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
|
|
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
|
-
|
|
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
|
-
|
|
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
|
-
#
|
|
1040
|
-
#
|
|
1041
|
-
#
|
|
1042
|
-
|
|
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
|
-
#
|
|
1305
|
-
#
|
|
1306
|
-
#
|
|
1307
|
-
|
|
1308
|
-
|
|
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
|
-
"""
|
|
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:
|
|
1406
|
-
non-latching guard would re-engage on the
|
|
1407
|
-
The follower re-arms only once the leader
|
|
1408
|
-
:data:`TORQUE_REARM_OPENNESS`, i.e. once the operator has
|
|
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
|
-
|
|
1438
|
-
|
|
1439
|
-
|
|
1440
|
-
|
|
1441
|
-
|
|
1442
|
-
|
|
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
|
|
27
|
-
|
|
28
|
-
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
|
|
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``
|
|
96
|
-
|
|
97
|
-
|
|
98
|
-
|
|
99
|
-
|
|
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
|
|
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``
|
|
154
|
-
(
|
|
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
|
|
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
|
|
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
|
|
360
|
-
#
|
|
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
|
|
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.
|
|
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
|
|
569
|
-
`torque_limit_nm` and
|
|
570
|
-
|
|
571
|
-
|
|
572
|
-
the
|
|
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=
|
|
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=
|
|
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=
|
|
11
|
+
litegrip/teleop.py,sha256=XyaG6DgFcMq6j1ABq0VlHPXAegOUuUEuKyZMPG21Sco,88372
|
|
12
12
|
litegrip/trajectory.py,sha256=n1m97jADWvyk6FdUQGA69g8D2mFwD0bg0o5Zh9Mrpgg,44081
|
|
13
|
-
litegrip/zenoh_link.py,sha256=
|
|
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.
|
|
22
|
-
litegrip_python-1.
|
|
23
|
-
litegrip_python-1.
|
|
24
|
-
litegrip_python-1.
|
|
25
|
-
litegrip_python-1.
|
|
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,,
|
|
File without changes
|
|
File without changes
|
|
File without changes
|