litegrip-python 1.2.1__py3-none-any.whl → 1.3.1__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/actions.py +112 -11
- litegrip/can/controller.py +50 -11
- 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.1.dist-info}/METADATA +33 -10
- {litegrip_python-1.2.1.dist-info → litegrip_python-1.3.1.dist-info}/RECORD +11 -11
- {litegrip_python-1.2.1.dist-info → litegrip_python-1.3.1.dist-info}/WHEEL +0 -0
- {litegrip_python-1.2.1.dist-info → litegrip_python-1.3.1.dist-info}/licenses/LICENSE +0 -0
- {litegrip_python-1.2.1.dist-info → litegrip_python-1.3.1.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/actions.py
CHANGED
|
@@ -46,6 +46,11 @@ ROS 2 桥接、RPC 服务、产品代码都能直接调 :attr:`LiteGrip.actions`
|
|
|
46
46
|
闭合侧有约 0.010 rad 的机械死区,慢速粘滑时单点忽大忽小。斜坡走完的保压段
|
|
47
47
|
不判(那时夹爪本来就该不动)。
|
|
48
48
|
|
|
49
|
+
带设定力的接近段还多一条**真停**条件:窗口平均速度低于
|
|
50
|
+
:attr:`MotionConfig.stall_speed_mm_s` 才算停住。这一段的实际速度本来就可能被
|
|
51
|
+
力矩预算和摩擦压到指令速度的 20% 以下,只按比例判会把「慢」当成「顶住」,
|
|
52
|
+
把纯力矩保力段接到还在走的夹爪上。
|
|
53
|
+
|
|
49
54
|
保力段(``grasp`` 的第二段)反过来:只下发前馈力矩,不给位置/速度增益。
|
|
50
55
|
力控要的是力,带增益就会随夹爪的位移衰减,详见
|
|
51
56
|
:meth:`GripperActions._hold_force`。
|
|
@@ -133,6 +138,16 @@ class MotionConfig:
|
|
|
133
138
|
stall_cycles: int = 5 # 窗口采样点数
|
|
134
139
|
stall_ratio: float = 0.2 # 窗口净位移 / 本该走的距离
|
|
135
140
|
stall_delta: float = 0.0015 # 阈值下限 rad
|
|
141
|
+
# 带设定力接近段判「真停」的速度下限 mm/s。这一段是位置帧,但预算和摩擦
|
|
142
|
+
# 会把实际速度压到指令的 20% 以下 —— 那不是接触。窗口平均速度必须低于
|
|
143
|
+
# 这个值才允许交接给纯力矩保力段。2026-11-18 台架:5 N 接近段约 8 mm/s,
|
|
144
|
+
# 0.25 s 只走 2 mm,被 20% 阈值判成堵转;保力段随后把它加速到 7 rad/s,
|
|
145
|
+
# 从 78 mm 冲到 26 mm 的工件上。
|
|
146
|
+
# 已知边界:自由蠕动低于这个值时仍可能被当成停住(2.5 N、约 1.9 mm/s
|
|
147
|
+
# 可复现)。2.5 N 低于 README 的 3 N 建议值(2.2 N 以下闭不上,2.2–3 N
|
|
148
|
+
# 勉强),属于不进保力段的边缘档位;支持范围内这条门槛留了 2 倍余量
|
|
149
|
+
# (5 N 接近段仍约 8 mm/s)。降到 0.5 mm/s 会让真接触也迟迟判不出来。
|
|
150
|
+
stall_speed_mm_s: float = 2.0
|
|
136
151
|
|
|
137
152
|
# ── 力矩 / 指令上限 ────────────────────────────────────────────────
|
|
138
153
|
max_lead_mm: float = 4.0 # 行进段领先上限 mm(≈ kp × 上限)
|
|
@@ -247,6 +262,15 @@ class MotionConfig:
|
|
|
247
262
|
monotonic_fn: Callable[[], float] = field(
|
|
248
263
|
default=time.monotonic, repr=False, compare=False)
|
|
249
264
|
|
|
265
|
+
def __post_init__(self) -> None:
|
|
266
|
+
"""拒绝会静默失效的交接速度门槛。"""
|
|
267
|
+
if (not math.isfinite(self.stall_speed_mm_s)
|
|
268
|
+
or self.stall_speed_mm_s < 0.0):
|
|
269
|
+
raise ValueError(
|
|
270
|
+
"MotionConfig.stall_speed_mm_s 必须是非负有限数:"
|
|
271
|
+
f"{self.stall_speed_mm_s!r}。负值会让窗口平均速度永远高于"
|
|
272
|
+
"门槛,接近段对任何速度都不判真停。")
|
|
273
|
+
|
|
250
274
|
|
|
251
275
|
# ═══════════════════════════════════════════════════════════════════════════
|
|
252
276
|
# 结果类型
|
|
@@ -822,7 +846,9 @@ class GripperActions:
|
|
|
822
846
|
它才一路走到底。
|
|
823
847
|
|
|
824
848
|
**交接只在「到位或顶上东西」时发生**(``reached or stalled``)。这两条
|
|
825
|
-
|
|
849
|
+
在真机上是同一件事的两面:夹爪都停着。带设定力的接近段里,「顶上」还要
|
|
850
|
+
过一条真停条件:窗口平均速度低于 :attr:`MotionConfig.stall_speed_mm_s`
|
|
851
|
+
—— 慢速蠕动不算接触(见该字段)。这不是保守,是唯一的正确条件 ——
|
|
826
852
|
保力段是纯力矩源,帧里没有位置项也没有速度项,把它接到一个**还在自由
|
|
827
853
|
行程里**的夹爪上,位置环一撤它就只有加速这一条路(2026-10-13 实测:
|
|
828
854
|
0.82 rad/s → 5.12 rad/s,312 mm/s,且到 ``hold_s`` 用完仍在加速)。
|
|
@@ -863,6 +889,9 @@ class GripperActions:
|
|
|
863
889
|
的力矩预算(:func:`force_approach_terms`)整段变成 NaN,NaN 的
|
|
864
890
|
``hold_s`` 会让保力段一帧不保就返回 —— 偏偏报「成功」。
|
|
865
891
|
一直保力请用 ``hold_s=0``,不是 ``inf``。
|
|
892
|
+
CommError: 接近段或进入保力段前要状态帧失败。保力**中途**的状态帧
|
|
893
|
+
失败不抛出:撤力后返回 ``ok=False``(``cycles=0``),避免把最后
|
|
894
|
+
那档力矩留在总线上。
|
|
866
895
|
"""
|
|
867
896
|
cfg = self.config
|
|
868
897
|
force = cfg.force_n if force_n is None else force_n
|
|
@@ -946,7 +975,25 @@ class GripperActions:
|
|
|
946
975
|
if req is not None:
|
|
947
976
|
return self._grasp_ends_with_release(req, move, force, cycles=0)
|
|
948
977
|
|
|
949
|
-
|
|
978
|
+
try:
|
|
979
|
+
hold = self._hold_force(force, hold_s, progress=progress)
|
|
980
|
+
except CommError as exc:
|
|
981
|
+
# 保力段每片都要刷新状态;总线在这期间断了。最后那档力矩不能
|
|
982
|
+
# 留在总线上,调用方也不该以为保力正常结束 —— 撤力后按失败返回。
|
|
983
|
+
log.warning("保力中断:%s —— 撤力并返回 ok=False", exc)
|
|
984
|
+
try:
|
|
985
|
+
self._release(move.state.position_rad)
|
|
986
|
+
except LiteGripError as rel_exc:
|
|
987
|
+
log.warning("保力中断后的撤力也失败:%s", rel_exc)
|
|
988
|
+
return GraspResult(
|
|
989
|
+
ok=False,
|
|
990
|
+
reached=move.reached,
|
|
991
|
+
stalled=move.stalled,
|
|
992
|
+
state=move.state,
|
|
993
|
+
target_rad=move.target_rad,
|
|
994
|
+
force_n=force,
|
|
995
|
+
cycles=0,
|
|
996
|
+
)
|
|
950
997
|
if hold.release_requested:
|
|
951
998
|
req = self._take_release_request()
|
|
952
999
|
if req is None: # 兜底:请求不该凭空消失
|
|
@@ -1343,7 +1390,11 @@ class GripperActions:
|
|
|
1343
1390
|
# 是一次位置运动,方向来自标定,未标定时拒绝。
|
|
1344
1391
|
if not req.abort:
|
|
1345
1392
|
_check_calibrated(gcfg)
|
|
1346
|
-
|
|
1393
|
+
# 急停不要求新状态:零增益帧里 q 不产生任何力,缓存位置就够。反过来,
|
|
1394
|
+
# 要求先答一帧会把撤力拖到重试之后,电机不答时甚至一帧都发不出去 ——
|
|
1395
|
+
# 总线还通、电机不说话,正是急停最需要撤力的工况。
|
|
1396
|
+
st = (g.get_state(wait=False) if req.abort
|
|
1397
|
+
else self._fresh_state())
|
|
1347
1398
|
pos = st.position_rad
|
|
1348
1399
|
|
|
1349
1400
|
if req.abort:
|
|
@@ -1464,6 +1515,40 @@ class GripperActions:
|
|
|
1464
1515
|
self._emit(pos, 0.0, 0.0, kp=0.0, kd=0.0)
|
|
1465
1516
|
return frames
|
|
1466
1517
|
|
|
1518
|
+
def _fresh_state(self) -> GripperState:
|
|
1519
|
+
"""Read a status snapshot that a frame actually answered, or refuse.
|
|
1520
|
+
|
|
1521
|
+
A DM motor answers control frames; when nothing is being sent it says
|
|
1522
|
+
nothing, and ``get_state(wait=True)`` returns the cache after ~50 ms.
|
|
1523
|
+
Planning a ramp from that cache is how a jaw that was moved while the
|
|
1524
|
+
bus was idle made the first frame demand the whole error: on 2026-11-18
|
|
1525
|
+
the cache said 78 mm, the jaws were at 26 mm, and ``open()`` left at
|
|
1526
|
+
23 rad/s out of a 20 mm/s move.
|
|
1527
|
+
|
|
1528
|
+
``refresh_status()`` (0xCC) asks an idle motor to speak. A cached
|
|
1529
|
+
snapshot — even one younger than :data:`STALE_AFTER_S` — is not enough
|
|
1530
|
+
to plan motion from: at 20 mm/s, half a second of silence is 10 mm of
|
|
1531
|
+
unknown. Retry the request a few times and raise :class:`CommError` if
|
|
1532
|
+
no frame answered.
|
|
1533
|
+
|
|
1534
|
+
Raises:
|
|
1535
|
+
CommError: the 0xCC request could not be sent, or the motor did not
|
|
1536
|
+
answer it with a status frame.
|
|
1537
|
+
"""
|
|
1538
|
+
sent_ok = False
|
|
1539
|
+
last: Optional[GripperState] = None
|
|
1540
|
+
for _ in range(3):
|
|
1541
|
+
sent_ok = self._g.refresh_status() or sent_ok
|
|
1542
|
+
last = self._g.get_state() # waits up to 50 ms for a frame
|
|
1543
|
+
if last.fresh:
|
|
1544
|
+
return last
|
|
1545
|
+
age = ("无" if last is None or last.data_age_s is None
|
|
1546
|
+
else "%.1f s" % last.data_age_s)
|
|
1547
|
+
raise CommError(
|
|
1548
|
+
"状态帧请求%s,电机没有回应(缓存 age=%s)—— 拒绝从缓存规划运动;"
|
|
1549
|
+
"检查 CAN 链路和电机供电。"
|
|
1550
|
+
% ("" if sent_ok else "发不出去", age))
|
|
1551
|
+
|
|
1467
1552
|
def _move_to_limit(
|
|
1468
1553
|
self,
|
|
1469
1554
|
toward: str,
|
|
@@ -1552,7 +1637,7 @@ class GripperActions:
|
|
|
1552
1637
|
target, limit, _margin_rad, _travel = limit_target(
|
|
1553
1638
|
gcfg, toward, cfg.margin)
|
|
1554
1639
|
|
|
1555
|
-
before =
|
|
1640
|
+
before = self._fresh_state()
|
|
1556
1641
|
dist_rad = target - before.position_rad
|
|
1557
1642
|
dist_mm = abs(dist_rad) * gcfg.rad_to_mm
|
|
1558
1643
|
sign = 1.0 if dist_rad >= 0 else -1.0
|
|
@@ -1592,11 +1677,15 @@ class GripperActions:
|
|
|
1592
1677
|
win_s = win * sample_every * interval
|
|
1593
1678
|
win_expect_rad = speed_rad_s * win_s
|
|
1594
1679
|
win_thresh_rad = max(cfg.stall_delta, cfg.stall_ratio * win_expect_rad)
|
|
1680
|
+
# 带设定力接近段的“真停”下限:窗口平均速度低于它才算停住。
|
|
1681
|
+
stall_speed_rad_s = cfg.stall_speed_mm_s / gcfg.rad_to_mm
|
|
1595
1682
|
# 帧数上界。不带力的移动按排程走,排程跑完就是它的终止条件之一;带设定
|
|
1596
1683
|
# 力的接近段不行 —— 领先被预算压小(见 force_approach_terms),夹爪追不
|
|
1597
1684
|
# 追得上排程由摩擦和读数延迟定,``ramp_steps`` 到了它可能还差几十毫米。
|
|
1598
1685
|
# 所以这一段的帧数只是兜底,上界取「按仍然算进展的最慢速度走完全程要几
|
|
1599
|
-
#
|
|
1686
|
+
# 帧」。比这根速度还慢、但窗口平均又高于 stall_speed_mm_s 的蠕动会走到
|
|
1687
|
+
# 这个上界,按「既没到位也没堵转」拒夹;只有低于 stall_speed_mm_s 的
|
|
1688
|
+
# 才算真停、才允许交接。
|
|
1600
1689
|
ceiling = total_steps
|
|
1601
1690
|
if force_n is not None:
|
|
1602
1691
|
floor_rad_s = win_thresh_rad / win_s if win_s > 0.0 else 0.0
|
|
@@ -1737,9 +1826,16 @@ class GripperActions:
|
|
|
1737
1826
|
# 夹爪到了那里就该顶着不动 —— 那里的「不动」正是我们要的堵转。而且
|
|
1738
1827
|
# 越位量(5% 行程)往往短于一个采样窗口,只在斜坡段判会永远判不到,
|
|
1739
1828
|
# 动作得白跑完保压段。
|
|
1829
|
+
# 带设定力的接近段:窗口位移小还不够 —— 实际速度被预算/摩擦压到
|
|
1830
|
+
# 指令的 20% 以下是常态,那不是停住。再看一眼窗口平均速度,低于
|
|
1831
|
+
# stall_speed_mm_s 才算真停,才允许交接给纯力矩保力段。
|
|
1832
|
+
moving = (force_n is not None and win_delta is not None
|
|
1833
|
+
and win_s > 0.0
|
|
1834
|
+
and win_delta / win_s > stall_speed_rad_s)
|
|
1740
1835
|
if (judge_stall and win_delta is not None
|
|
1741
1836
|
and (press or in_ramp)
|
|
1742
|
-
and win_delta < win_thresh_rad
|
|
1837
|
+
and win_delta < win_thresh_rad
|
|
1838
|
+
and not moving):
|
|
1743
1839
|
# 「没动」不等于「顶到工件」:带设定力时,从起点起就没挪过位的
|
|
1744
1840
|
# 停住也可能是这一段压不出脱离静摩擦的力矩。这一条不能当接触,
|
|
1745
1841
|
# 否则交接给纯力矩的保力段就是一次带力冲程(见
|
|
@@ -1763,8 +1859,9 @@ class GripperActions:
|
|
|
1763
1859
|
else:
|
|
1764
1860
|
stalled = True
|
|
1765
1861
|
log.info("堵转:最近 %d 次采样(%.2f s)净位移仅 %.5f rad "
|
|
1766
|
-
"(< %.5f),停在 %+.5f rad",
|
|
1767
|
-
win, win_s, win_delta, win_thresh_rad,
|
|
1862
|
+
"(< %.5f,平均 %.2f mm/s),停在 %+.5f rad",
|
|
1863
|
+
win, win_s, win_delta, win_thresh_rad,
|
|
1864
|
+
win_delta / win_s * gcfg.rad_to_mm, pos)
|
|
1768
1865
|
break
|
|
1769
1866
|
|
|
1770
1867
|
# 行进段堵转力矩保护(≈7 N):与位置窗口判据**互补** —— 位置窗口对
|
|
@@ -1790,7 +1887,7 @@ class GripperActions:
|
|
|
1790
1887
|
if protected:
|
|
1791
1888
|
self._release(pos)
|
|
1792
1889
|
|
|
1793
|
-
st =
|
|
1890
|
+
st = self._fresh_state() # 要一帧答过的读数再判到位
|
|
1794
1891
|
reached = abs(st.position_rad - target) < cfg.reach_tol
|
|
1795
1892
|
if press:
|
|
1796
1893
|
# 顶到限位(堵转 + 停在标定限位附近)才算成功;半路撞工件
|
|
@@ -1859,6 +1956,10 @@ class GripperActions:
|
|
|
1859
1956
|
:attr:`MotionConfig.hold_interval` 的 0.2 s 片查 —— 请求一到就停保力,
|
|
1860
1957
|
结果里 ``release_requested=True``,张开/急停由 ``grasp()`` 接手。
|
|
1861
1958
|
|
|
1959
|
+
Raises:
|
|
1960
|
+
CommError: 状态帧在保力期间没有得到回应。``grasp()`` 会撤力并把它
|
|
1961
|
+
转成 ``ok=False``,不把异常抛给调用方。
|
|
1962
|
+
|
|
1862
1963
|
Returns:
|
|
1863
1964
|
:class:`HoldResult`。
|
|
1864
1965
|
"""
|
|
@@ -1880,7 +1981,7 @@ class GripperActions:
|
|
|
1880
1981
|
# 的片里就成了 4 N 一级的台阶,不是爬升。
|
|
1881
1982
|
step_nm = cfg.force_ramp_n_s * cfg.frame_interval * UnitConversion.N_TO_NM
|
|
1882
1983
|
cycles = 0
|
|
1883
|
-
st =
|
|
1984
|
+
st = self._fresh_state()
|
|
1884
1985
|
pos = st.position_rad
|
|
1885
1986
|
# 进入保力:从飞行中的力矩接着爬,交接才连续。飞行力矩已经越过设定值时
|
|
1886
1987
|
# 直接从设定值起步(那一步是往下走到设定值,不是往上,不构成冲击)。
|
|
@@ -1916,7 +2017,7 @@ class GripperActions:
|
|
|
1916
2017
|
self._emit(pos, 0.0, tau_cmd, kp=0.0, kd=0.0)
|
|
1917
2018
|
cycles += 1
|
|
1918
2019
|
|
|
1919
|
-
st =
|
|
2020
|
+
st = self._fresh_state()
|
|
1920
2021
|
if st.error_code != 1:
|
|
1921
2022
|
log.warning("保力中断:状态帧 err=%d"
|
|
1922
2023
|
"(1=使能中;0=被禁用;其他=故障)", st.error_code)
|
litegrip/can/controller.py
CHANGED
|
@@ -19,7 +19,7 @@ import time
|
|
|
19
19
|
from typing import Dict, Optional
|
|
20
20
|
|
|
21
21
|
from ..exceptions import CANTimeoutError
|
|
22
|
-
from .transport import CanTransport
|
|
22
|
+
from .transport import CanFrame, CanTransport
|
|
23
23
|
from .protocol import (
|
|
24
24
|
BROADCAST_ID,
|
|
25
25
|
ControlMode,
|
|
@@ -42,6 +42,11 @@ from .motor import MotorState, MotorParams, MotorType
|
|
|
42
42
|
|
|
43
43
|
log = logging.getLogger("litegrip.can.controller")
|
|
44
44
|
|
|
45
|
+
# ``poll()`` drains the RX queue so the newest state wins. Cap the drain so a
|
|
46
|
+
# bus that never goes quiet cannot hold a caller indefinitely; the remainder
|
|
47
|
+
# stays queued for the next poll.
|
|
48
|
+
MAX_DRAIN_FRAMES = 256
|
|
49
|
+
|
|
45
50
|
|
|
46
51
|
class MotorController:
|
|
47
52
|
"""Manages DM motors over a CAN transport.
|
|
@@ -240,17 +245,45 @@ class MotorController:
|
|
|
240
245
|
|
|
241
246
|
# ── polling ──────────────────────────────────────────────────────────
|
|
242
247
|
|
|
243
|
-
def poll(self, timeout_s: float = 0.0
|
|
244
|
-
|
|
245
|
-
|
|
248
|
+
def poll(self, timeout_s: float = 0.0,
|
|
249
|
+
max_frames: int = MAX_DRAIN_FRAMES) -> Optional[MotorState]:
|
|
250
|
+
"""Read every status frame already queued; the newest state wins.
|
|
251
|
+
|
|
252
|
+
The motor answers each control frame with a status frame. A caller
|
|
253
|
+
that sends a burst (a force-hold slice, a ramp step) and polls once per
|
|
254
|
+
burst leaves the rest of the answers in the socket buffer; reading one
|
|
255
|
+
frame then returns an *old* position, and the age is stamped when the
|
|
256
|
+
frame is read — so the stale reading looks fresh. On 2026-11-18 that
|
|
257
|
+
made a position ramp start from the pose the jaws had at the start of a
|
|
258
|
+
hold: the first frame demanded ~0.7 rad of error and the jaws left at
|
|
259
|
+
23 rad/s out of a 20 mm/s move.
|
|
260
|
+
|
|
261
|
+
``timeout_s`` only applies to the wait for the first frame; the drain
|
|
262
|
+
after it is non-blocking. Parameter responses and foreign frames are
|
|
263
|
+
discarded without stopping the drain. At most ``max_frames`` frames
|
|
264
|
+
are drained per call (default :data:`MAX_DRAIN_FRAMES`), so a bus that
|
|
265
|
+
never goes quiet cannot hold the caller indefinitely; anything left
|
|
266
|
+
stays queued for the next poll.
|
|
246
267
|
|
|
247
268
|
Returns:
|
|
248
|
-
The MotorState
|
|
269
|
+
The MotorState updated by the newest relevant frame, or None if no
|
|
270
|
+
relevant frame arrived.
|
|
249
271
|
"""
|
|
250
|
-
|
|
251
|
-
|
|
252
|
-
|
|
253
|
-
|
|
272
|
+
updated: Optional[MotorState] = None
|
|
273
|
+
first = True
|
|
274
|
+
for _ in range(max(1, int(max_frames))):
|
|
275
|
+
frame = self._transport.recv(
|
|
276
|
+
timeout_s=timeout_s if first else 0.0)
|
|
277
|
+
first = False
|
|
278
|
+
if frame is None:
|
|
279
|
+
return updated
|
|
280
|
+
motor = self._apply_status_frame(frame)
|
|
281
|
+
if motor is not None:
|
|
282
|
+
updated = motor
|
|
283
|
+
return updated
|
|
284
|
+
|
|
285
|
+
def _apply_status_frame(self, frame: CanFrame) -> Optional[MotorState]:
|
|
286
|
+
"""Decode one received frame into its motor state, if it is one."""
|
|
254
287
|
# Match frame.can_id against registered motors' mst_id
|
|
255
288
|
motor = self._motors.get(frame.can_id)
|
|
256
289
|
if motor is None:
|
|
@@ -306,14 +339,20 @@ class MotorController:
|
|
|
306
339
|
timeout_s: float = 1.0) -> bool:
|
|
307
340
|
"""Poll until at least one new status frame arrives for the given motor.
|
|
308
341
|
|
|
342
|
+
``poll()`` drains every queued frame and reports only the motor updated
|
|
343
|
+
by the newest one. On a bus with several registered motors that is not
|
|
344
|
+
necessarily ``motor`` — another motor's frame may be drained last — so
|
|
345
|
+
the target's ``rx_count``, not the returned motor, decides whether its
|
|
346
|
+
update was seen.
|
|
347
|
+
|
|
309
348
|
Returns:
|
|
310
349
|
True if a frame was received, False on timeout.
|
|
311
350
|
"""
|
|
312
351
|
prev_rx = motor.rx_count
|
|
313
352
|
deadline = time.monotonic() + timeout_s
|
|
314
353
|
while time.monotonic() < deadline:
|
|
315
|
-
|
|
316
|
-
if
|
|
354
|
+
self.poll(timeout_s=0.01)
|
|
355
|
+
if motor.rx_count > prev_rx:
|
|
317
356
|
return True
|
|
318
357
|
time.sleep(0.001)
|
|
319
358
|
return False
|
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:
|