litegrip-python 1.3.0__tar.gz → 1.3.1__tar.gz

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.
Files changed (49) hide show
  1. {litegrip_python-1.3.0/src/litegrip_python.egg-info → litegrip_python-1.3.1}/PKG-INFO +6 -3
  2. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/README.md +5 -2
  3. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/pyproject.toml +1 -1
  4. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/actions.py +112 -11
  5. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/can/controller.py +50 -11
  6. {litegrip_python-1.3.0 → litegrip_python-1.3.1/src/litegrip_python.egg-info}/PKG-INFO +6 -3
  7. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip_python.egg-info/SOURCES.txt +1 -0
  8. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_actions.py +149 -5
  9. litegrip_python-1.3.1/tests/test_can_poll_drain.py +151 -0
  10. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/LICENSE +0 -0
  11. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/setup.cfg +0 -0
  12. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/__init__.py +0 -0
  13. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/calibration_normal.json +0 -0
  14. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/calibration_reverse.json +0 -0
  15. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/can/__init__.py +0 -0
  16. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/can/motor.py +0 -0
  17. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/can/protocol.py +0 -0
  18. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/can/transport.py +0 -0
  19. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/constants.py +0 -0
  20. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/exceptions.py +0 -0
  21. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/factory_calibration.json +0 -0
  22. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/gripper.py +0 -0
  23. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/models.py +0 -0
  24. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/protocols/__init__.py +0 -0
  25. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/protocols/can_bus.py +0 -0
  26. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/py.typed +0 -0
  27. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/teleop.py +0 -0
  28. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/trajectory.py +0 -0
  29. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip/zenoh_link.py +0 -0
  30. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip_python.egg-info/dependency_links.txt +0 -0
  31. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip_python.egg-info/requires.txt +0 -0
  32. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/src/litegrip_python.egg-info/top_level.txt +0 -0
  33. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_bus_limits.py +0 -0
  34. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_calibration.py +0 -0
  35. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_can_stream_silence.py +0 -0
  36. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_enable.py +0 -0
  37. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_example_hold_force.py +0 -0
  38. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_example_teleop.py +0 -0
  39. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_examples_bootstrap.py +0 -0
  40. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_iface_mtu.py +0 -0
  41. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_protocol.py +0 -0
  42. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_send_failure_reports_its_cause.py +0 -0
  43. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_state_staleness.py +0 -0
  44. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_teleop.py +0 -0
  45. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_trajectory.py +0 -0
  46. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_transport_send.py +0 -0
  47. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_version.py +0 -0
  48. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_write_zero.py +0 -0
  49. {litegrip_python-1.3.0 → litegrip_python-1.3.1}/tests/test_zenoh_link.py +0 -0
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.4
2
2
  Name: litegrip-python
3
- Version: 1.3.0
3
+ Version: 1.3.1
4
4
  Summary: Python SDK for the LiteGrip lightweight robotic gripper series
5
5
  Author: NEXFORM ROBOTICS
6
6
  License-Expression: Apache-2.0
@@ -202,7 +202,7 @@ young. Ask both when the difference matters.
202
202
  | `open(speed_mm_s=None, *, progress=None)` → `MoveResult` | Ramps toward the calibrated open-side stop. With `GripperConfig.work_stroke_mm` set it stops at that opening instead — the shipped calibration sets `80.0` against the unit's 85 mm of travel, so a plain `open()` ends 5 mm short of the open end. |
203
203
  | `close(speed_mm_s=None, *, progress=None)` → `MoveResult` | The same, toward the closed side. `work_stroke_mm` is an open-side number and does not change where a close ends. |
204
204
  | `home()` → `MoveResult` | Ramps to the calibrated closed position. |
205
- | `grasp(force_n=None, hold_s=0.0, *, progress=None)` → `GraspResult` | Closes onto the object — or onto the closed-side calibrated stop if there is nothing there — then ramps to and holds `force_n` N. The closing leg carries `force_n` as its torque budget, so meeting the object presses no harder than the setpoint. The hold takes over only once the jaw is stopped: it is `stalled=True` (something is between the jaws) or `reached=True` (the jaws closed empty). A stop that came **without the jaw ever moving from where the approach began**, with a budget below `MotionConfig.breakaway_nm`, is not treated as contact — the budget simply cannot break static friction — so the call releases in place and returns `no_bite=True` instead of handing over the hold. `hold_s=0` holds until a fault, the runaway watch, Ctrl+C, or a `release()` / `request_release()` from another thread — which ends the grasp with `released=True`. `force_n=None` uses `MotionConfig.force_n` (20 N). |
205
+ | `grasp(force_n=None, hold_s=0.0, *, progress=None)` → `GraspResult` | Closes onto the object — or onto the closed-side calibrated stop if there is nothing there — then ramps to and holds `force_n` N. The closing leg carries `force_n` as its torque budget, so meeting the object presses no harder than the setpoint. The hold takes over only once the jaw is stopped: it is `stalled=True` (something is between the jaws) or `reached=True` (the jaws closed empty). In the force approach `stalled=True` also requires the window's average speed to be under `MotionConfig.stall_speed_mm_s` (2 mm/s), so a jaw that is still creeping is not mistaken for contact and handed to the torque-only hold. A stop that came **without the jaw ever moving from where the approach began**, with a budget below `MotionConfig.breakaway_nm`, is not treated as contact — the budget simply cannot break static friction — so the call releases in place and returns `no_bite=True` instead of handing over the hold. `hold_s=0` holds until a fault, the runaway watch, Ctrl+C, or a `release()` / `request_release()` from another thread — which ends the grasp with `released=True`. `force_n=None` uses `MotionConfig.force_n` (20 N). |
206
206
  | `release(clearance_mm=10.0, *, abort=False, timeout_s=None)` → `ReleaseResult` | Ends an in-progress `grasp()` and opens the jaws from the pose where the grip was declared, by `clearance_mm` more, clamped to the calibrated open stop. Safe to call from any thread while `grasp(hold_s=0)` is blocked in another: it queues the request, the grasp thread performs the opening, and the call returns that outcome. With no grasp running it opens from the current position in the calling thread. `abort=True` is the E-stop form: torque off, no opening move, `ok=False`. A second call during the same grasp joins the first. |
207
207
  | `request_release(clearance_mm=10.0, *, abort=False)` → `bool` | Non-blocking form of `release()`: records the request if a grasp is running and returns `True`, else `False`. Never sends a frame, so it is safe for a UI thread, a signal handler or an E-stop callback. The grasp thread acts on it before its next frame (within 5 ms). |
208
208
  | `goto(position_mm, kp=None, kd=None, duration=0.5)` → `MoveResult` | Ramp to a position in millimetres. |
@@ -415,7 +415,10 @@ What it *means*, and what to check, is per action:
415
415
  A fourth combination is a failure, and new in this version:
416
416
 
417
417
  - **`grasp()` with `ok=False, reached=False, stalled=False`** means the closing leg ran out of
418
- frames without reaching the stop and without stalling — the jaw is still in free travel. The
418
+ frames without reaching the stop and without stalling — the jaw is still in free travel. A
419
+ slow creep lands here too: the window's net displacement is small, but its average speed is
420
+ still above `MotionConfig.stall_speed_mm_s` (2 mm/s), so it never counts as a stop and the leg
421
+ runs to its frame ceiling. The
419
422
  hold is *not* entered: its frames carry no position and no velocity term, so handing a moving
420
423
  jaw over to them leaves the feed-forward torque as the only thing acting on it, and the jaw
421
424
  accelerates until the runaway watch releases it. The call releases in place instead and reports
@@ -174,7 +174,7 @@ young. Ask both when the difference matters.
174
174
  | `open(speed_mm_s=None, *, progress=None)` → `MoveResult` | Ramps toward the calibrated open-side stop. With `GripperConfig.work_stroke_mm` set it stops at that opening instead — the shipped calibration sets `80.0` against the unit's 85 mm of travel, so a plain `open()` ends 5 mm short of the open end. |
175
175
  | `close(speed_mm_s=None, *, progress=None)` → `MoveResult` | The same, toward the closed side. `work_stroke_mm` is an open-side number and does not change where a close ends. |
176
176
  | `home()` → `MoveResult` | Ramps to the calibrated closed position. |
177
- | `grasp(force_n=None, hold_s=0.0, *, progress=None)` → `GraspResult` | Closes onto the object — or onto the closed-side calibrated stop if there is nothing there — then ramps to and holds `force_n` N. The closing leg carries `force_n` as its torque budget, so meeting the object presses no harder than the setpoint. The hold takes over only once the jaw is stopped: it is `stalled=True` (something is between the jaws) or `reached=True` (the jaws closed empty). A stop that came **without the jaw ever moving from where the approach began**, with a budget below `MotionConfig.breakaway_nm`, is not treated as contact — the budget simply cannot break static friction — so the call releases in place and returns `no_bite=True` instead of handing over the hold. `hold_s=0` holds until a fault, the runaway watch, Ctrl+C, or a `release()` / `request_release()` from another thread — which ends the grasp with `released=True`. `force_n=None` uses `MotionConfig.force_n` (20 N). |
177
+ | `grasp(force_n=None, hold_s=0.0, *, progress=None)` → `GraspResult` | Closes onto the object — or onto the closed-side calibrated stop if there is nothing there — then ramps to and holds `force_n` N. The closing leg carries `force_n` as its torque budget, so meeting the object presses no harder than the setpoint. The hold takes over only once the jaw is stopped: it is `stalled=True` (something is between the jaws) or `reached=True` (the jaws closed empty). In the force approach `stalled=True` also requires the window's average speed to be under `MotionConfig.stall_speed_mm_s` (2 mm/s), so a jaw that is still creeping is not mistaken for contact and handed to the torque-only hold. A stop that came **without the jaw ever moving from where the approach began**, with a budget below `MotionConfig.breakaway_nm`, is not treated as contact — the budget simply cannot break static friction — so the call releases in place and returns `no_bite=True` instead of handing over the hold. `hold_s=0` holds until a fault, the runaway watch, Ctrl+C, or a `release()` / `request_release()` from another thread — which ends the grasp with `released=True`. `force_n=None` uses `MotionConfig.force_n` (20 N). |
178
178
  | `release(clearance_mm=10.0, *, abort=False, timeout_s=None)` → `ReleaseResult` | Ends an in-progress `grasp()` and opens the jaws from the pose where the grip was declared, by `clearance_mm` more, clamped to the calibrated open stop. Safe to call from any thread while `grasp(hold_s=0)` is blocked in another: it queues the request, the grasp thread performs the opening, and the call returns that outcome. With no grasp running it opens from the current position in the calling thread. `abort=True` is the E-stop form: torque off, no opening move, `ok=False`. A second call during the same grasp joins the first. |
179
179
  | `request_release(clearance_mm=10.0, *, abort=False)` → `bool` | Non-blocking form of `release()`: records the request if a grasp is running and returns `True`, else `False`. Never sends a frame, so it is safe for a UI thread, a signal handler or an E-stop callback. The grasp thread acts on it before its next frame (within 5 ms). |
180
180
  | `goto(position_mm, kp=None, kd=None, duration=0.5)` → `MoveResult` | Ramp to a position in millimetres. |
@@ -387,7 +387,10 @@ What it *means*, and what to check, is per action:
387
387
  A fourth combination is a failure, and new in this version:
388
388
 
389
389
  - **`grasp()` with `ok=False, reached=False, stalled=False`** means the closing leg ran out of
390
- frames without reaching the stop and without stalling — the jaw is still in free travel. The
390
+ frames without reaching the stop and without stalling — the jaw is still in free travel. A
391
+ slow creep lands here too: the window's net displacement is small, but its average speed is
392
+ still above `MotionConfig.stall_speed_mm_s` (2 mm/s), so it never counts as a stop and the leg
393
+ runs to its frame ceiling. The
391
394
  hold is *not* entered: its frames carry no position and no velocity term, so handing a moving
392
395
  jaw over to them leaves the feed-forward torque as the only thing acting on it, and the jaw
393
396
  accelerates until the runaway watch releases it. The call releases in place instead and reports
@@ -14,7 +14,7 @@ name = "litegrip-python"
14
14
  # Python repositories the placeholder has to be plain `0.0.0`. The nearby
15
15
  # `0.0.0+semantic-release` spelling is valid PEP 440 but PyPI rejects local version
16
16
  # labels, so it would only move the failure to upload time.
17
- version = "1.3.0"
17
+ version = "1.3.1"
18
18
  description = "Python SDK for the LiteGrip lightweight robotic gripper series"
19
19
  readme = "README.md"
20
20
  requires-python = ">=3.10"
@@ -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
- hold = self._hold_force(force, hold_s, progress=progress)
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
- st = g.get_state()
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 = g.get_state()
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
- # 帧」:比这更慢时堵转判据必然已经在 win_s 内触发,这个上界到不了。
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, pos)
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 = g.get_state() # 阻塞等一帧新状态再判到位
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 = g.get_state()
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 = g.get_state()
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)
@@ -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) -> Optional[MotorState]:
244
- """Poll for one CAN frame. If it's a status frame for a registered
245
- motor, decode it and update that MotorState.
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 that was updated, or None if no relevant frame arrived.
269
+ The MotorState updated by the newest relevant frame, or None if no
270
+ relevant frame arrived.
249
271
  """
250
- frame = self._transport.recv(timeout_s=timeout_s)
251
- if frame is None:
252
- return None
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
- updated = self.poll(timeout_s=0.01)
316
- if updated is motor and motor.rx_count > prev_rx:
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
@@ -1,6 +1,6 @@
1
1
  Metadata-Version: 2.4
2
2
  Name: litegrip-python
3
- Version: 1.3.0
3
+ Version: 1.3.1
4
4
  Summary: Python SDK for the LiteGrip lightweight robotic gripper series
5
5
  Author: NEXFORM ROBOTICS
6
6
  License-Expression: Apache-2.0
@@ -202,7 +202,7 @@ young. Ask both when the difference matters.
202
202
  | `open(speed_mm_s=None, *, progress=None)` → `MoveResult` | Ramps toward the calibrated open-side stop. With `GripperConfig.work_stroke_mm` set it stops at that opening instead — the shipped calibration sets `80.0` against the unit's 85 mm of travel, so a plain `open()` ends 5 mm short of the open end. |
203
203
  | `close(speed_mm_s=None, *, progress=None)` → `MoveResult` | The same, toward the closed side. `work_stroke_mm` is an open-side number and does not change where a close ends. |
204
204
  | `home()` → `MoveResult` | Ramps to the calibrated closed position. |
205
- | `grasp(force_n=None, hold_s=0.0, *, progress=None)` → `GraspResult` | Closes onto the object — or onto the closed-side calibrated stop if there is nothing there — then ramps to and holds `force_n` N. The closing leg carries `force_n` as its torque budget, so meeting the object presses no harder than the setpoint. The hold takes over only once the jaw is stopped: it is `stalled=True` (something is between the jaws) or `reached=True` (the jaws closed empty). A stop that came **without the jaw ever moving from where the approach began**, with a budget below `MotionConfig.breakaway_nm`, is not treated as contact — the budget simply cannot break static friction — so the call releases in place and returns `no_bite=True` instead of handing over the hold. `hold_s=0` holds until a fault, the runaway watch, Ctrl+C, or a `release()` / `request_release()` from another thread — which ends the grasp with `released=True`. `force_n=None` uses `MotionConfig.force_n` (20 N). |
205
+ | `grasp(force_n=None, hold_s=0.0, *, progress=None)` → `GraspResult` | Closes onto the object — or onto the closed-side calibrated stop if there is nothing there — then ramps to and holds `force_n` N. The closing leg carries `force_n` as its torque budget, so meeting the object presses no harder than the setpoint. The hold takes over only once the jaw is stopped: it is `stalled=True` (something is between the jaws) or `reached=True` (the jaws closed empty). In the force approach `stalled=True` also requires the window's average speed to be under `MotionConfig.stall_speed_mm_s` (2 mm/s), so a jaw that is still creeping is not mistaken for contact and handed to the torque-only hold. A stop that came **without the jaw ever moving from where the approach began**, with a budget below `MotionConfig.breakaway_nm`, is not treated as contact — the budget simply cannot break static friction — so the call releases in place and returns `no_bite=True` instead of handing over the hold. `hold_s=0` holds until a fault, the runaway watch, Ctrl+C, or a `release()` / `request_release()` from another thread — which ends the grasp with `released=True`. `force_n=None` uses `MotionConfig.force_n` (20 N). |
206
206
  | `release(clearance_mm=10.0, *, abort=False, timeout_s=None)` → `ReleaseResult` | Ends an in-progress `grasp()` and opens the jaws from the pose where the grip was declared, by `clearance_mm` more, clamped to the calibrated open stop. Safe to call from any thread while `grasp(hold_s=0)` is blocked in another: it queues the request, the grasp thread performs the opening, and the call returns that outcome. With no grasp running it opens from the current position in the calling thread. `abort=True` is the E-stop form: torque off, no opening move, `ok=False`. A second call during the same grasp joins the first. |
207
207
  | `request_release(clearance_mm=10.0, *, abort=False)` → `bool` | Non-blocking form of `release()`: records the request if a grasp is running and returns `True`, else `False`. Never sends a frame, so it is safe for a UI thread, a signal handler or an E-stop callback. The grasp thread acts on it before its next frame (within 5 ms). |
208
208
  | `goto(position_mm, kp=None, kd=None, duration=0.5)` → `MoveResult` | Ramp to a position in millimetres. |
@@ -415,7 +415,10 @@ What it *means*, and what to check, is per action:
415
415
  A fourth combination is a failure, and new in this version:
416
416
 
417
417
  - **`grasp()` with `ok=False, reached=False, stalled=False`** means the closing leg ran out of
418
- frames without reaching the stop and without stalling — the jaw is still in free travel. The
418
+ frames without reaching the stop and without stalling — the jaw is still in free travel. A
419
+ slow creep lands here too: the window's net displacement is small, but its average speed is
420
+ still above `MotionConfig.stall_speed_mm_s` (2 mm/s), so it never counts as a stop and the leg
421
+ runs to its frame ceiling. The
419
422
  hold is *not* entered: its frames carry no position and no velocity term, so handing a moving
420
423
  jaw over to them leaves the feed-forward torque as the only thing acting on it, and the jaw
421
424
  accelerates until the runaway watch releases it. The call releases in place instead and reports
@@ -29,6 +29,7 @@ src/litegrip_python.egg-info/top_level.txt
29
29
  tests/test_actions.py
30
30
  tests/test_bus_limits.py
31
31
  tests/test_calibration.py
32
+ tests/test_can_poll_drain.py
32
33
  tests/test_can_stream_silence.py
33
34
  tests/test_enable.py
34
35
  tests/test_example_hold_force.py
@@ -5,13 +5,14 @@ from __future__ import annotations
5
5
  import threading
6
6
  import time
7
7
  import unittest
8
+ from unittest import mock
8
9
 
9
10
  import _sdkpath # noqa: F401
10
- from litegrip import (CommandError, GraspResult, GripperConfig, GripperState,
11
- LiteGrip, MotionConfig, ReleaseResult, UnitConversion,
12
- clamp_to_travel, closing_travel_excursion_rad,
13
- force_approach_terms, limit_target, press_target,
14
- work_limit_target)
11
+ from litegrip import (CommError, CommandError, GraspResult, GripperConfig,
12
+ GripperState, LiteGrip, MotionConfig, ReleaseResult,
13
+ UnitConversion, clamp_to_travel,
14
+ closing_travel_excursion_rad, force_approach_terms,
15
+ limit_target, press_target, work_limit_target)
15
16
  from litegrip.actions import GripperActions, MoveResult
16
17
 
17
18
  from fake_can import (DT, POS_CLOSED_RAD, POS_OPEN_RAD, RAD_TO_MM,
@@ -989,6 +990,73 @@ class TestGraspHandsOverOnlyOnContact(unittest.TestCase):
989
990
  self.assertTrue(any(f.kp == 0.0 for f in fake.frames), "没有撤力帧")
990
991
 
991
992
 
993
+ class TestForceApproachNeedsATrueStop(unittest.TestCase):
994
+ """2026-11-18 台架:5 N 接近段实际只有约 8 mm/s,0.25 s 走 2 mm。
995
+
996
+ 20% 的窗口阈值把「慢」判成「顶上东西」,保力段随后在自由行程里把夹爪
997
+ 加速到 7 rad/s,从 78 mm 冲到 26 mm 的工件上。交接前必须看到**真停**:
998
+ 窗口平均速度低于 ``MotionConfig.stall_speed_mm_s``。
999
+ """
1000
+
1001
+ def test_a_slow_creep_is_not_handed_over_mid_travel(self):
1002
+ object_rad = (POS_OPEN_RAD + POS_CLOSED_RAD) / 2.0
1003
+ motion = MotionConfig(sleep_fn=lambda _: None,
1004
+ monotonic_fn=tick_clock(0.1))
1005
+ crawl_rad_s = 0.109 # ≈ 8 mm/s at RAD_TO_MM
1006
+ g, fake = make_gripper(block_rad=object_rad, stops=True,
1007
+ motion=motion, speed_cap_rad_s=crawl_rad_s)
1008
+
1009
+ g.grasp(force_n=5.0, hold_s=0.2)
1010
+
1011
+ holds = [f for f in fake.frames if f.tau_ff != 0.0]
1012
+ self.assertTrue(holds, "没有进入保力段")
1013
+ first = holds[0].pos_after
1014
+ self.assertLess(
1015
+ abs(first - object_rad), g.motion_config.reach_tol,
1016
+ "保力段在离工件 %.2f mm 处接手了 —— 又是自由行程里交接" %
1017
+ (abs(first - object_rad) * RAD_TO_MM))
1018
+
1019
+ def test_a_slow_creep_without_contact_never_enters_the_hold(self):
1020
+ """没有工件、只是被摩擦压在 8 mm/s:走到帧数上界按拒夹收尾。
1021
+
1022
+ 这条钉的是「不交接」本身,而不只是「交接点提前」:慢速蠕动既没到位
1023
+ 也没真停,保力段一帧都不许发 —— 它是纯力矩源,接过去只会继续加速。
1024
+ """
1025
+ motion = MotionConfig(sleep_fn=lambda _: None,
1026
+ monotonic_fn=tick_clock(0.1))
1027
+ g, fake = make_gripper(stops=True, motion=motion,
1028
+ speed_cap_rad_s=0.109) # ≈ 8 mm/s
1029
+
1030
+ res = g.grasp(force_n=5.0, hold_s=0.2)
1031
+
1032
+ self.assertFalse(res.ok, res)
1033
+ self.assertFalse(res.stalled, res)
1034
+ self.assertFalse(res.reached, res)
1035
+ self.assertEqual(res.cycles, 0)
1036
+ self.assertEqual([f for f in fake.frames if f.tau_ff != 0.0], [],
1037
+ "慢速蠕动被交给了纯力矩的保力段")
1038
+
1039
+
1040
+ class TestMotionRefreshesAnIdleBus(unittest.TestCase):
1041
+ """空闲总线上的读数只在状态帧里更新;动作不能从手扳之前的缓存起跳。"""
1042
+
1043
+ def test_open_does_not_start_from_a_position_read_before_the_bus_went_idle(self):
1044
+ g, fake = make_gripper(stops=True, freeze_position_while_idle=True)
1045
+ g.get_state() # 缓存停在张开侧
1046
+ fake.delivers = False
1047
+ actual = POS_CLOSED_RAD - 0.4 # 总线空闲时被手扳到中间
1048
+ fake.motor.pos = actual
1049
+
1050
+ g.open()
1051
+
1052
+ first = fake.frames[0]
1053
+ self.assertLess(
1054
+ abs(first.q - actual), 0.01,
1055
+ "open 从缓存位置起跳 %.1f mm(实际在 %.1f mm)" %
1056
+ (abs(first.q - actual) * RAD_TO_MM,
1057
+ abs(actual - fake.motor.pos) * RAD_TO_MM))
1058
+
1059
+
992
1060
  class TestRelease(unittest.TestCase):
993
1061
  """松开:跨线程结束 ``grasp()``、从交接点张余量、急停、不重复张开。
994
1062
 
@@ -2163,5 +2231,81 @@ class TestUncalibratedConfigRefusesMotion(unittest.TestCase):
2163
2231
  self.assertEqual(fake.frames, [])
2164
2232
 
2165
2233
 
2234
+ class TestFreshStateRefusesAnUnansweredCache(unittest.TestCase):
2235
+ """缓存还新不等于位置可信:0xCC 没人答,就不许规划运动。"""
2236
+
2237
+ def test_a_recent_cache_is_not_enough_when_nothing_answers(self):
2238
+ g, fake = make_gripper(stops=True)
2239
+ g.get_state() # 先让缓存里有一帧、age 很小
2240
+ fake.delivers = False # 总线安静
2241
+ fake.refresh_ok = False # 0xCC 也发不出去
2242
+
2243
+ with self.assertRaises(CommError):
2244
+ g.open()
2245
+
2246
+
2247
+ class TestAbortReleaseNeedsNoFreshFrame(unittest.TestCase):
2248
+ """急停只发一帧 kp=kd=tau=0;电机不答状态帧也必须撤力。"""
2249
+
2250
+ def test_abort_releases_when_the_motor_does_not_answer(self):
2251
+ g, fake = make_gripper(stops=True)
2252
+ g.get_state() # 缓存里有一帧
2253
+ fake.delivers = False # 电机不主动开口
2254
+ fake.refresh_status = lambda: True # 0xCC 写成功,但电机不答
2255
+
2256
+ res = g.release(abort=True)
2257
+
2258
+ self.assertFalse(res.ok)
2259
+ self.assertTrue(
2260
+ any(f.kp == 0.0 and f.kd == 0.0 and f.tau_ff == 0.0
2261
+ for f in fake.frames),
2262
+ "急停没有发出零力矩帧 —— 最后一档力矩留在了总线上")
2263
+
2264
+
2265
+ class TestFinalReachUsesAFreshFrame(unittest.TestCase):
2266
+ """移动结束判 reached/ok 前必须再要一帧,不能拿循环里的缓存。"""
2267
+
2268
+ def test_the_end_of_a_move_requests_a_status_frame(self):
2269
+ g, fake = make_gripper(stops=True)
2270
+
2271
+ res = g.open(SPEED_MM_S)
2272
+
2273
+ self.assertTrue(res.ok, res)
2274
+ self.assertGreaterEqual(
2275
+ fake.refresh_calls, 2,
2276
+ "移动结束时没有重新要状态帧 —— reached/ok 用的是缓存")
2277
+
2278
+
2279
+ class TestGraspHoldCommError(unittest.TestCase):
2280
+ """保力中途断总线:撤力、返回 ok=False,不把异常抛给调用方。"""
2281
+
2282
+ def test_a_mid_hold_comm_error_releases_and_reports_failure(self):
2283
+ block = (POS_OPEN_RAD + POS_CLOSED_RAD) / 2.0
2284
+ g, fake = make_gripper(block_rad=block)
2285
+
2286
+ with mock.patch.object(g._actions, "_hold_force",
2287
+ side_effect=CommError("bus down")):
2288
+ res = g.grasp(force_n=5.0, hold_s=0.2)
2289
+
2290
+ self.assertFalse(res.ok, res)
2291
+ self.assertEqual(res.cycles, 0)
2292
+ self.assertTrue(res.stalled, "接近段应当先夹到工件")
2293
+ self.assertTrue(
2294
+ any(f.kp == 0.0 and f.kd == 0.0 and f.tau_ff == 0.0
2295
+ for f in fake.frames),
2296
+ "保力中断后没有撤力帧 —— 最后一档力矩留在了总线上")
2297
+ self.assertTrue(g._actions._hold_done.is_set(),
2298
+ "grasp() 的 finally 没有收尾")
2299
+
2300
+
2301
+ class TestMotionConfigValidation(unittest.TestCase):
2302
+
2303
+ def test_non_finite_or_negative_stall_speed_is_rejected(self):
2304
+ for value in (-1.0, float("nan"), float("inf")):
2305
+ with self.subTest(value=value):
2306
+ with self.assertRaises(ValueError):
2307
+ MotionConfig(stall_speed_mm_s=value)
2308
+
2309
+
2166
2310
  if __name__ == "__main__":
2167
2311
  unittest.main()
@@ -0,0 +1,151 @@
1
+ """``MotorController.poll`` must drain the RX queue; the newest frame wins.
2
+
3
+ The motor answers every control frame with a status frame. A caller that
4
+ sends a burst (a force-hold slice, a ramp step) and polls once per burst leaves
5
+ the answers queued; reading one frame returns an old position, and the age is
6
+ stamped when it is read, so the stale reading looks fresh. On 2026-11-18 the
7
+ ramp after a hold started from the hold's first pose (78 mm) while the jaws
8
+ were at 26 mm — the first frame demanded ~0.7 rad, and the drive left at
9
+ 23 rad/s out of a 20 mm/s move.
10
+ """
11
+
12
+ from __future__ import annotations
13
+
14
+ import unittest
15
+
16
+ import _sdkpath # noqa: F401
17
+ from litegrip.can.controller import MAX_DRAIN_FRAMES, MotorController
18
+ from litegrip.can.protocol import float_to_uint
19
+ from litegrip.can.transport import CanFrame
20
+
21
+ MST_ID = 0x18
22
+ CAN_ID = 0x08
23
+
24
+
25
+ def status_frame(position_rad: float, err: int = 1,
26
+ can_id: int = CAN_ID, mst_id: int = MST_ID) -> CanFrame:
27
+ """Build one DM status frame (q, dq=0, tau=0)."""
28
+ q_uint = float_to_uint(position_rad, -12.5, 12.5, 16)
29
+ dq_uint = float_to_uint(0.0, -30.0, 30.0, 12)
30
+ tau_uint = float_to_uint(0.0, -10.0, 10.0, 12)
31
+ data = bytes([
32
+ ((err & 0x0F) << 4) | (can_id & 0x0F),
33
+ (q_uint >> 8) & 0xFF,
34
+ q_uint & 0xFF,
35
+ (dq_uint >> 4) & 0xFF,
36
+ ((dq_uint & 0x0F) << 4) | ((tau_uint >> 8) & 0x0F),
37
+ tau_uint & 0xFF,
38
+ 30, 30,
39
+ ])
40
+ return CanFrame(can_id=mst_id, data=data)
41
+
42
+
43
+ class QueueTransport:
44
+ """A transport that hands out pre-queued frames, then goes quiet."""
45
+
46
+ def __init__(self, frames):
47
+ self._frames = list(frames)
48
+ self.sent = []
49
+
50
+ def recv(self, timeout_s: float = 0.0):
51
+ return self._frames.pop(0) if self._frames else None
52
+
53
+ def send(self, can_id, data, timeout=0.1):
54
+ self.sent.append((can_id, data))
55
+
56
+ def send_multi(self, can_id, data, count=1, interval_s=0.002):
57
+ for _ in range(count):
58
+ self.send(can_id, data)
59
+
60
+ def set_id_filter(self, ids):
61
+ pass
62
+
63
+
64
+ class TestPollDrainsTheQueue(unittest.TestCase):
65
+
66
+ def make_controller(self, frames):
67
+ transport = QueueTransport(frames)
68
+ controller = MotorController(transport)
69
+ motor = controller.add_motor(can_id=CAN_ID, mst_id=MST_ID)
70
+ return controller, motor
71
+
72
+ def test_the_newest_queued_frame_wins(self):
73
+ controller, motor = self.make_controller([
74
+ status_frame(-1.0),
75
+ status_frame(-0.5),
76
+ status_frame(-0.2),
77
+ ])
78
+
79
+ updated = controller.poll()
80
+
81
+ self.assertIs(updated, motor)
82
+ self.assertEqual(motor.rx_count, 3, "队列没有被排空")
83
+ self.assertAlmostEqual(motor.position, -0.2, places=3,
84
+ msg="读到的还是队列里最旧的那一帧")
85
+
86
+ def test_foreign_frames_between_status_frames_do_not_stop_the_drain(self):
87
+ foreign = CanFrame(can_id=0x123, data=bytes(8))
88
+ controller, motor = self.make_controller([
89
+ status_frame(-1.0),
90
+ foreign,
91
+ status_frame(-0.3),
92
+ ])
93
+
94
+ controller.poll()
95
+
96
+ self.assertEqual(motor.rx_count, 2)
97
+ self.assertAlmostEqual(motor.position, -0.3, places=3)
98
+
99
+ def test_a_single_frame_is_still_reported(self):
100
+ controller, motor = self.make_controller([status_frame(0.25)])
101
+
102
+ updated = controller.poll()
103
+
104
+ self.assertIs(updated, motor)
105
+ self.assertEqual(motor.rx_count, 1)
106
+ self.assertAlmostEqual(motor.position, 0.25, places=3)
107
+
108
+ def test_an_empty_queue_returns_none(self):
109
+ controller, _ = self.make_controller([])
110
+
111
+ self.assertIsNone(controller.poll())
112
+
113
+
114
+ class TestPollUntilSeesTheTargetUpdate(unittest.TestCase):
115
+ """``poll()`` reports only the newest motor; ``poll_until`` must not."""
116
+
117
+ def test_the_target_counts_even_when_another_motor_is_newest(self):
118
+ transport = QueueTransport([
119
+ status_frame(-1.0, can_id=0x08, mst_id=0x18),
120
+ status_frame(-0.5, can_id=0x09, mst_id=0x19),
121
+ ])
122
+ controller = MotorController(transport)
123
+ target = controller.add_motor(can_id=0x08, mst_id=0x18)
124
+ controller.add_motor(can_id=0x09, mst_id=0x19)
125
+
126
+ self.assertTrue(controller.poll_until(target, timeout_s=0.05),
127
+ "目标电机的帧先到、另一个电机最后被排空,更新丢了")
128
+ self.assertEqual(target.rx_count, 1)
129
+
130
+
131
+ class TestDrainIsBounded(unittest.TestCase):
132
+ """A bus that never goes quiet must not hold ``poll`` indefinitely."""
133
+
134
+ def test_the_drain_stops_at_the_cap_and_leaves_the_rest_queued(self):
135
+ transport = QueueTransport(
136
+ [status_frame(0.0) for _ in range(MAX_DRAIN_FRAMES + 5)])
137
+ controller = MotorController(transport)
138
+ motor = controller.add_motor(can_id=CAN_ID, mst_id=MST_ID)
139
+
140
+ controller.poll()
141
+
142
+ self.assertEqual(motor.rx_count, MAX_DRAIN_FRAMES)
143
+
144
+ controller.poll()
145
+
146
+ self.assertEqual(motor.rx_count, MAX_DRAIN_FRAMES + 5,
147
+ "未排空的帧没有留给下一次 poll")
148
+
149
+
150
+ if __name__ == "__main__":
151
+ unittest.main()
File without changes