diff --git a/README.md b/README.md index a9590bf..c8867a3 100644 --- a/README.md +++ b/README.md @@ -174,7 +174,7 @@ young. Ask both when the difference matters. | `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. | | `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. | | `home()` → `MoveResult` | Ramps to the calibrated closed position. | -| `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). | +| `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). | | `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. | | `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). | | `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: A fourth combination is a failure, and new in this version: - **`grasp()` with `ok=False, reached=False, stalled=False`** means the closing leg ran out of - frames without reaching the stop and without stalling — the jaw is still in free travel. The + frames without reaching the stop and without stalling — the jaw is still in free travel. A + slow creep lands here too: the window's net displacement is small, but its average speed is + still above `MotionConfig.stall_speed_mm_s` (2 mm/s), so it never counts as a stop and the leg + runs to its frame ceiling. The hold is *not* entered: its frames carry no position and no velocity term, so handing a moving jaw over to them leaves the feed-forward torque as the only thing acting on it, and the jaw accelerates until the runaway watch releases it. The call releases in place instead and reports diff --git a/README.zh-CN.md b/README.zh-CN.md index 58e8ad7..8849b22 100644 --- a/README.zh-CN.md +++ b/README.zh-CN.md @@ -163,7 +163,7 @@ print(state.position_mm, state.force_n, state.timestamp) | `open(speed_mm_s=None, *, progress=None)` → `MoveResult` | 爬坡走向标定的张开侧限位。若 `GripperConfig.work_stroke_mm` 有值,则停在那段行程处 —— 出厂标定设的是 `80.0`,对着这台机器 85 mm 的行程,所以裸调 `open()` 会在距张开端 5 mm 处停下。 | | `close(speed_mm_s=None, *, progress=None)` → `MoveResult` | 一样,方向朝闭合侧。`work_stroke_mm` 是张开侧的数字,不改变 `close()` 的落点。 | | `home()` → `MoveResult` | 爬坡回到标定的闭合位置。 | -| `grasp(force_n=None, hold_s=0.0, *, progress=None)` → `GraspResult` | 闭到工件上(中间没东西就闭到闭合侧标定限位),再爬到 `force_n` N 并保持。闭合段就把 `force_n` 当力矩预算,所以顶到工件时压得不会比设定值更狠。保力只在爪子**停下来**之后接手:`stalled=True`(爪子间夹着东西)或 `reached=True`(空爪闭到底)。但**从起点就没动过**就停下、且预算低于 `MotionConfig.breakaway_nm` 时不算接触 —— 那是预算压不出静摩擦 —— 这种情形调用原地撤力、返回 `no_bite=True`,不进保力段。`hold_s=0` 表示保持到故障、失控监视触发或 Ctrl+C。`force_n=None` 用 `MotionConfig.force_n`(20 N)。 | +| `grasp(force_n=None, hold_s=0.0, *, progress=None)` → `GraspResult` | 闭到工件上(中间没东西就闭到闭合侧标定限位),再爬到 `force_n` N 并保持。闭合段就把 `force_n` 当力矩预算,所以顶到工件时压得不会比设定值更狠。保力只在爪子**停下来**之后接手:`stalled=True`(爪子间夹着东西)或 `reached=True`(空爪闭到底)。带力接近段里 `stalled=True` 还要求窗口平均速度低于 `MotionConfig.stall_speed_mm_s`(2 mm/s)—— 还在慢速蠕动的夹爪不算接触,不交给纯力矩的保力段。但**从起点就没动过**就停下、且预算低于 `MotionConfig.breakaway_nm` 时不算接触 —— 那是预算压不出静摩擦 —— 这种情形调用原地撤力、返回 `no_bite=True`,不进保力段。`hold_s=0` 表示保持到故障、失控监视触发或 Ctrl+C。`force_n=None` 用 `MotionConfig.force_n`(20 N)。 | | `goto(position_mm, kp=None, kd=None, duration=0.5)` → `MoveResult` | 爬坡走到毫米目标。 | | `goto_rad(position_rad, kp=None, kd=None, dq_target=0.0, tau_feedforward=0.0, duration=0.5)` → `MoveResult` | 一样的,单位 rad。`dq_target` 与 `tau_feedforward` 必须是 0:速度前馈由斜坡按自己的排程给出,所以这两个值一律拒绝并抛 `CommandError`,不是静默忽略。 | | `move_to(target_rad, kp=None, kd=None, tau_feedforward=0.0, duration=1.0)` → `MoveResult` | 和 `goto_rad` 一样,只是 `duration` 的默认值更长。 | @@ -333,7 +333,9 @@ print(abs(cfg.pos_open_rad - cfg.pos_closed_rad) * cfg.rad_to_mm) # 行程,m 第四种组合是失败,也是这一版新增的: - **`grasp()` 返回 `ok=False, reached=False, stalled=False`**,意思是闭合段帧数用完了,既没 - 走到限位也没堵转 —— 爪子还在自由行程里。这一路**不会**进保力段:保力帧既没有位置项也没有 + 走到限位也没堵转 —— 爪子还在自由行程里。慢速蠕动也归这一路:窗口净位移虽小,平均速度仍 + 高于 `MotionConfig.stall_speed_mm_s`(2 mm/s),不算真停,闭合段一直走到帧数上界、按失败 + 收尾。这一路**不会**进保力段:保力帧既没有位置项也没有 速度项,把一个还在走的爪子交给它们,压在它身上的就只剩前馈力矩,它会一路加速直到失控监视 撤力。此时调用改为原地撤力,报 `cycles=0`。什么也没夹住。见到它就说明闭合段被饿着了 —— 读数过期,或者机构自身预算吃不掉的摩擦。 diff --git a/src/litegrip/actions.py b/src/litegrip/actions.py index 5c1ddc7..e4a74ae 100644 --- a/src/litegrip/actions.py +++ b/src/litegrip/actions.py @@ -46,6 +46,11 @@ 闭合侧有约 0.010 rad 的机械死区,慢速粘滑时单点忽大忽小。斜坡走完的保压段 不判(那时夹爪本来就该不动)。 +带设定力的接近段还多一条**真停**条件:窗口平均速度低于 +:attr:`MotionConfig.stall_speed_mm_s` 才算停住。这一段的实际速度本来就可能被 +力矩预算和摩擦压到指令速度的 20% 以下,只按比例判会把「慢」当成「顶住」, +把纯力矩保力段接到还在走的夹爪上。 + 保力段(``grasp`` 的第二段)反过来:只下发前馈力矩,不给位置/速度增益。 力控要的是力,带增益就会随夹爪的位移衰减,详见 :meth:`GripperActions._hold_force`。 @@ -133,6 +138,16 @@ class MotionConfig: stall_cycles: int = 5 # 窗口采样点数 stall_ratio: float = 0.2 # 窗口净位移 / 本该走的距离 stall_delta: float = 0.0015 # 阈值下限 rad + # 带设定力接近段判「真停」的速度下限 mm/s。这一段是位置帧,但预算和摩擦 + # 会把实际速度压到指令的 20% 以下 —— 那不是接触。窗口平均速度必须低于 + # 这个值才允许交接给纯力矩保力段。2026-11-18 台架:5 N 接近段约 8 mm/s, + # 0.25 s 只走 2 mm,被 20% 阈值判成堵转;保力段随后把它加速到 7 rad/s, + # 从 78 mm 冲到 26 mm 的工件上。 + # 已知边界:自由蠕动低于这个值时仍可能被当成停住(2.5 N、约 1.9 mm/s + # 可复现)。2.5 N 低于 README 的 3 N 建议值(2.2 N 以下闭不上,2.2–3 N + # 勉强),属于不进保力段的边缘档位;支持范围内这条门槛留了 2 倍余量 + # (5 N 接近段仍约 8 mm/s)。降到 0.5 mm/s 会让真接触也迟迟判不出来。 + stall_speed_mm_s: float = 2.0 # ── 力矩 / 指令上限 ──────────────────────────────────────────────── max_lead_mm: float = 4.0 # 行进段领先上限 mm(≈ kp × 上限) @@ -247,6 +262,15 @@ class MotionConfig: monotonic_fn: Callable[[], float] = field( default=time.monotonic, repr=False, compare=False) + def __post_init__(self) -> None: + """拒绝会静默失效的交接速度门槛。""" + if (not math.isfinite(self.stall_speed_mm_s) + or self.stall_speed_mm_s < 0.0): + raise ValueError( + "MotionConfig.stall_speed_mm_s 必须是非负有限数:" + f"{self.stall_speed_mm_s!r}。负值会让窗口平均速度永远高于" + "门槛,接近段对任何速度都不判真停。") + # ═══════════════════════════════════════════════════════════════════════════ # 结果类型 @@ -822,7 +846,9 @@ def grasp( 它才一路走到底。 **交接只在「到位或顶上东西」时发生**(``reached or stalled``)。这两条 - 在真机上是同一件事的两面:夹爪都停着。这不是保守,是唯一的正确条件 —— + 在真机上是同一件事的两面:夹爪都停着。带设定力的接近段里,「顶上」还要 + 过一条真停条件:窗口平均速度低于 :attr:`MotionConfig.stall_speed_mm_s` + —— 慢速蠕动不算接触(见该字段)。这不是保守,是唯一的正确条件 —— 保力段是纯力矩源,帧里没有位置项也没有速度项,把它接到一个**还在自由 行程里**的夹爪上,位置环一撤它就只有加速这一条路(2026-10-13 实测: 0.82 rad/s → 5.12 rad/s,312 mm/s,且到 ``hold_s`` 用完仍在加速)。 @@ -863,6 +889,9 @@ def grasp( 的力矩预算(:func:`force_approach_terms`)整段变成 NaN,NaN 的 ``hold_s`` 会让保力段一帧不保就返回 —— 偏偏报「成功」。 一直保力请用 ``hold_s=0``,不是 ``inf``。 + CommError: 接近段或进入保力段前要状态帧失败。保力**中途**的状态帧 + 失败不抛出:撤力后返回 ``ok=False``(``cycles=0``),避免把最后 + 那档力矩留在总线上。 """ cfg = self.config force = cfg.force_n if force_n is None else force_n @@ -946,7 +975,25 @@ def grasp( if req is not None: return self._grasp_ends_with_release(req, move, force, cycles=0) - hold = self._hold_force(force, hold_s, progress=progress) + try: + hold = self._hold_force(force, hold_s, progress=progress) + except CommError as exc: + # 保力段每片都要刷新状态;总线在这期间断了。最后那档力矩不能 + # 留在总线上,调用方也不该以为保力正常结束 —— 撤力后按失败返回。 + log.warning("保力中断:%s —— 撤力并返回 ok=False", exc) + try: + self._release(move.state.position_rad) + except LiteGripError as rel_exc: + log.warning("保力中断后的撤力也失败:%s", rel_exc) + return GraspResult( + ok=False, + reached=move.reached, + stalled=move.stalled, + state=move.state, + target_rad=move.target_rad, + force_n=force, + cycles=0, + ) if hold.release_requested: req = self._take_release_request() if req is None: # 兜底:请求不该凭空消失 @@ -1343,7 +1390,11 @@ def _do_release(self, req: _ReleaseRequest) -> ReleaseResult: # 是一次位置运动,方向来自标定,未标定时拒绝。 if not req.abort: _check_calibrated(gcfg) - st = g.get_state() + # 急停不要求新状态:零增益帧里 q 不产生任何力,缓存位置就够。反过来, + # 要求先答一帧会把撤力拖到重试之后,电机不答时甚至一帧都发不出去 —— + # 总线还通、电机不说话,正是急停最需要撤力的工况。 + st = (g.get_state(wait=False) if req.abort + else self._fresh_state()) pos = st.position_rad if req.abort: @@ -1464,6 +1515,40 @@ def _release(self, pos: float) -> int: self._emit(pos, 0.0, 0.0, kp=0.0, kd=0.0) return frames + def _fresh_state(self) -> GripperState: + """Read a status snapshot that a frame actually answered, or refuse. + + A DM motor answers control frames; when nothing is being sent it says + nothing, and ``get_state(wait=True)`` returns the cache after ~50 ms. + Planning a ramp from that cache is how a jaw that was moved while the + bus was idle made the first frame demand the whole error: on 2026-11-18 + the cache said 78 mm, the jaws were at 26 mm, and ``open()`` left at + 23 rad/s out of a 20 mm/s move. + + ``refresh_status()`` (0xCC) asks an idle motor to speak. A cached + snapshot — even one younger than :data:`STALE_AFTER_S` — is not enough + to plan motion from: at 20 mm/s, half a second of silence is 10 mm of + unknown. Retry the request a few times and raise :class:`CommError` if + no frame answered. + + Raises: + CommError: the 0xCC request could not be sent, or the motor did not + answer it with a status frame. + """ + sent_ok = False + last: Optional[GripperState] = None + for _ in range(3): + sent_ok = self._g.refresh_status() or sent_ok + last = self._g.get_state() # waits up to 50 ms for a frame + if last.fresh: + return last + age = ("无" if last is None or last.data_age_s is None + else "%.1f s" % last.data_age_s) + raise CommError( + "状态帧请求%s,电机没有回应(缓存 age=%s)—— 拒绝从缓存规划运动;" + "检查 CAN 链路和电机供电。" + % ("" if sent_ok else "发不出去", age)) + def _move_to_limit( self, toward: str, @@ -1552,7 +1637,7 @@ def _move_to_limit( target, limit, _margin_rad, _travel = limit_target( gcfg, toward, cfg.margin) - before = g.get_state() + before = self._fresh_state() dist_rad = target - before.position_rad dist_mm = abs(dist_rad) * gcfg.rad_to_mm sign = 1.0 if dist_rad >= 0 else -1.0 @@ -1592,11 +1677,15 @@ def _move_to_limit( win_s = win * sample_every * interval win_expect_rad = speed_rad_s * win_s win_thresh_rad = max(cfg.stall_delta, cfg.stall_ratio * win_expect_rad) + # 带设定力接近段的“真停”下限:窗口平均速度低于它才算停住。 + stall_speed_rad_s = cfg.stall_speed_mm_s / gcfg.rad_to_mm # 帧数上界。不带力的移动按排程走,排程跑完就是它的终止条件之一;带设定 # 力的接近段不行 —— 领先被预算压小(见 force_approach_terms),夹爪追不 # 追得上排程由摩擦和读数延迟定,``ramp_steps`` 到了它可能还差几十毫米。 # 所以这一段的帧数只是兜底,上界取「按仍然算进展的最慢速度走完全程要几 - # 帧」:比这更慢时堵转判据必然已经在 win_s 内触发,这个上界到不了。 + # 帧」。比这根速度还慢、但窗口平均又高于 stall_speed_mm_s 的蠕动会走到 + # 这个上界,按「既没到位也没堵转」拒夹;只有低于 stall_speed_mm_s 的 + # 才算真停、才允许交接。 ceiling = total_steps if force_n is not None: floor_rad_s = win_thresh_rad / win_s if win_s > 0.0 else 0.0 @@ -1737,9 +1826,16 @@ def _move_to_limit( # 夹爪到了那里就该顶着不动 —— 那里的「不动」正是我们要的堵转。而且 # 越位量(5% 行程)往往短于一个采样窗口,只在斜坡段判会永远判不到, # 动作得白跑完保压段。 + # 带设定力的接近段:窗口位移小还不够 —— 实际速度被预算/摩擦压到 + # 指令的 20% 以下是常态,那不是停住。再看一眼窗口平均速度,低于 + # stall_speed_mm_s 才算真停,才允许交接给纯力矩保力段。 + moving = (force_n is not None and win_delta is not None + and win_s > 0.0 + and win_delta / win_s > stall_speed_rad_s) if (judge_stall and win_delta is not None and (press or in_ramp) - and win_delta < win_thresh_rad): + and win_delta < win_thresh_rad + and not moving): # 「没动」不等于「顶到工件」:带设定力时,从起点起就没挪过位的 # 停住也可能是这一段压不出脱离静摩擦的力矩。这一条不能当接触, # 否则交接给纯力矩的保力段就是一次带力冲程(见 @@ -1763,8 +1859,9 @@ def _move_to_limit( else: stalled = True log.info("堵转:最近 %d 次采样(%.2f s)净位移仅 %.5f rad " - "(< %.5f),停在 %+.5f rad", - win, win_s, win_delta, win_thresh_rad, pos) + "(< %.5f,平均 %.2f mm/s),停在 %+.5f rad", + win, win_s, win_delta, win_thresh_rad, + win_delta / win_s * gcfg.rad_to_mm, pos) break # 行进段堵转力矩保护(≈7 N):与位置窗口判据**互补** —— 位置窗口对 @@ -1790,7 +1887,7 @@ def _move_to_limit( if protected: self._release(pos) - st = g.get_state() # 阻塞等一帧新状态再判到位 + st = self._fresh_state() # 要一帧答过的读数再判到位 reached = abs(st.position_rad - target) < cfg.reach_tol if press: # 顶到限位(堵转 + 停在标定限位附近)才算成功;半路撞工件 @@ -1859,6 +1956,10 @@ def _hold_force( :attr:`MotionConfig.hold_interval` 的 0.2 s 片查 —— 请求一到就停保力, 结果里 ``release_requested=True``,张开/急停由 ``grasp()`` 接手。 + Raises: + CommError: 状态帧在保力期间没有得到回应。``grasp()`` 会撤力并把它 + 转成 ``ok=False``,不把异常抛给调用方。 + Returns: :class:`HoldResult`。 """ @@ -1880,7 +1981,7 @@ def _hold_force( # 的片里就成了 4 N 一级的台阶,不是爬升。 step_nm = cfg.force_ramp_n_s * cfg.frame_interval * UnitConversion.N_TO_NM cycles = 0 - st = g.get_state() + st = self._fresh_state() pos = st.position_rad # 进入保力:从飞行中的力矩接着爬,交接才连续。飞行力矩已经越过设定值时 # 直接从设定值起步(那一步是往下走到设定值,不是往上,不构成冲击)。 @@ -1916,7 +2017,7 @@ def _hold_force( self._emit(pos, 0.0, tau_cmd, kp=0.0, kd=0.0) cycles += 1 - st = g.get_state() + st = self._fresh_state() if st.error_code != 1: log.warning("保力中断:状态帧 err=%d" "(1=使能中;0=被禁用;其他=故障)", st.error_code) diff --git a/src/litegrip/can/controller.py b/src/litegrip/can/controller.py index 4dfd3f2..9902ba4 100644 --- a/src/litegrip/can/controller.py +++ b/src/litegrip/can/controller.py @@ -19,7 +19,7 @@ from typing import Dict, Optional from ..exceptions import CANTimeoutError -from .transport import CanTransport +from .transport import CanFrame, CanTransport from .protocol import ( BROADCAST_ID, ControlMode, @@ -42,6 +42,11 @@ log = logging.getLogger("litegrip.can.controller") +# ``poll()`` drains the RX queue so the newest state wins. Cap the drain so a +# bus that never goes quiet cannot hold a caller indefinitely; the remainder +# stays queued for the next poll. +MAX_DRAIN_FRAMES = 256 + class MotorController: """Manages DM motors over a CAN transport. @@ -240,17 +245,45 @@ def save_params(self, motor: MotorState) -> None: # ── polling ────────────────────────────────────────────────────────── - def poll(self, timeout_s: float = 0.0) -> Optional[MotorState]: - """Poll for one CAN frame. If it's a status frame for a registered - motor, decode it and update that MotorState. + def poll(self, timeout_s: float = 0.0, + max_frames: int = MAX_DRAIN_FRAMES) -> Optional[MotorState]: + """Read every status frame already queued; the newest state wins. + + The motor answers each control frame with a status frame. A caller + that sends a burst (a force-hold slice, a ramp step) and polls once per + burst leaves the rest of the answers in the socket buffer; reading one + frame then returns an *old* position, and the age is stamped when the + frame is read — so the stale reading looks fresh. On 2026-11-18 that + made a position ramp start from the pose the jaws had at the start of a + hold: the first frame demanded ~0.7 rad of error and the jaws left at + 23 rad/s out of a 20 mm/s move. + + ``timeout_s`` only applies to the wait for the first frame; the drain + after it is non-blocking. Parameter responses and foreign frames are + discarded without stopping the drain. At most ``max_frames`` frames + are drained per call (default :data:`MAX_DRAIN_FRAMES`), so a bus that + never goes quiet cannot hold the caller indefinitely; anything left + stays queued for the next poll. Returns: - The MotorState that was updated, or None if no relevant frame arrived. + The MotorState updated by the newest relevant frame, or None if no + relevant frame arrived. """ - frame = self._transport.recv(timeout_s=timeout_s) - if frame is None: - return None - + updated: Optional[MotorState] = None + first = True + for _ in range(max(1, int(max_frames))): + frame = self._transport.recv( + timeout_s=timeout_s if first else 0.0) + first = False + if frame is None: + return updated + motor = self._apply_status_frame(frame) + if motor is not None: + updated = motor + return updated + + def _apply_status_frame(self, frame: CanFrame) -> Optional[MotorState]: + """Decode one received frame into its motor state, if it is one.""" # Match frame.can_id against registered motors' mst_id motor = self._motors.get(frame.can_id) if motor is None: @@ -306,14 +339,20 @@ def poll_until(self, motor: MotorState, timeout_s: float = 1.0) -> bool: """Poll until at least one new status frame arrives for the given motor. + ``poll()`` drains every queued frame and reports only the motor updated + by the newest one. On a bus with several registered motors that is not + necessarily ``motor`` — another motor's frame may be drained last — so + the target's ``rx_count``, not the returned motor, decides whether its + update was seen. + Returns: True if a frame was received, False on timeout. """ prev_rx = motor.rx_count deadline = time.monotonic() + timeout_s while time.monotonic() < deadline: - updated = self.poll(timeout_s=0.01) - if updated is motor and motor.rx_count > prev_rx: + self.poll(timeout_s=0.01) + if motor.rx_count > prev_rx: return True time.sleep(0.001) return False diff --git a/tests/fake_can.py b/tests/fake_can.py index bdb3d03..afd9930 100644 --- a/tests/fake_can.py +++ b/tests/fake_can.py @@ -82,10 +82,14 @@ def __init__(self, pos: float = 0.0, block_rad: Optional[float] = None, yield_rad_s: float = 0.0, yield_tau_nm: float = 0.0, creep_rad_s_per_nm: float = 0.0, report_lag_ticks: int = 0, - breakaway_tau_nm: float = 0.0): + breakaway_tau_nm: float = 0.0, + speed_cap_rad_s: Optional[float] = None): # 静摩擦的脱离力矩 Nm:站在原地的夹爪要帧里力矩过这个门槛才动。 # 0 = 没有静摩擦(默认,别的用例逐位不变)。 self.breakaway_tau_nm = breakaway_tau_nm + # 速度上限 rad/s。设置后模拟带力接近段被摩擦/预算压慢的工况: + # 指令 50 mm/s,实际只能爬。None = 只有 VMAX。 + self.speed_cap_rad_s = speed_cap_rad_s # 读数延迟(帧)。真机上 ``get_state(wait=False)`` 拿的是缓存读数,可能 # 落后几帧;而力控接近段的指令是 ``min(排程, 读数 + 领先上限)``,领先只 # 有零点几毫米时,``速度 × 延迟`` 就把它吃光了 —— 夹爪于是推不动排程。 @@ -166,7 +170,9 @@ def step(self, q: float, kp: float, dq: float, tau_ff: float, dt: float) -> None if (self.breakaway_tau_nm > 0.0 and self.vel == 0.0 and abs(kp * (q - self.pos) + tau_ff) < self.breakaway_tau_nm): v = 0.0 - v = max(-self.VMAX, min(self.VMAX, v)) + vmax = (self.VMAX if self.speed_cap_rad_s is None + else min(self.VMAX, self.speed_cap_rad_s)) + v = max(-vmax, min(vmax, v)) new = self.pos + v * dt if self.block_rad is not None: if self._block_dir > 0: @@ -206,7 +212,9 @@ def __init__(self, pos: float = 0.0, block_rad: Optional[float] = None, yield_rad_s: float = 0.0, yield_tau_nm: float = 0.0, creep_rad_s_per_nm: float = 0.0, report_lag_ticks: int = 0, - breakaway_tau_nm: float = 0.0): + breakaway_tau_nm: float = 0.0, + speed_cap_rad_s: Optional[float] = None, + freeze_position_while_idle: bool = False): self.motor = FakeMotor(pos=pos, block_rad=block_rad, sticky_rad=sticky_rad, err=err, limit_lo=limit_lo, limit_hi=limit_hi, @@ -214,7 +222,13 @@ def __init__(self, pos: float = 0.0, block_rad: Optional[float] = None, yield_tau_nm=yield_tau_nm, creep_rad_s_per_nm=creep_rad_s_per_nm, report_lag_ticks=report_lag_ticks, - breakaway_tau_nm=breakaway_tau_nm) + breakaway_tau_nm=breakaway_tau_nm, + speed_cap_rad_s=speed_cap_rad_s) + #: True = 没有帧到达时 ``get_position`` 不露真值,而是停在最后一次 + #: 收到的读数。真机就是这样:读数只在状态帧里更新,总线空闲时不会 + #: 自己变。默认 False,别的用例不受影响。 + self._freeze_position = freeze_position_while_idle + self._position_cache = self.motor.reported_pos() self.frames: List[Frame] = [] self.initialize_results = list(initialize_results or []) self.initialize_calls = 0 @@ -299,6 +313,8 @@ def _frame_available(self) -> bool: self.refresh_pending = False self.last_update = time.monotonic() self.last_frame_time = time.time() + if self._freeze_position: + self._position_cache = self.motor.reported_pos() return True return False @@ -310,6 +326,8 @@ def refresh_status(self) -> bool: return self.refresh_ok def get_position(self) -> float: + if self._freeze_position: + return self._position_cache return self.motor.reported_pos() def get_velocity(self) -> float: @@ -384,6 +402,8 @@ def make_gripper( creep_rad_s_per_nm: float = 0.0, report_lag_ticks: int = 0, breakaway_tau_nm: float = 0.0, + speed_cap_rad_s: Optional[float] = None, + freeze_position_while_idle: bool = False, ): """造一个「已连接、已使能、已标定」的 LiteGrip,底层换成 :class:`FakeLiteGripCAN`。 @@ -438,7 +458,9 @@ def make_gripper( yield_tau_nm=yield_tau_nm, creep_rad_s_per_nm=creep_rad_s_per_nm, report_lag_ticks=report_lag_ticks, - breakaway_tau_nm=breakaway_tau_nm) + breakaway_tau_nm=breakaway_tau_nm, + speed_cap_rad_s=speed_cap_rad_s, + freeze_position_while_idle=freeze_position_while_idle) g._can = fake g._connected = True g._enabled = True diff --git a/tests/test_actions.py b/tests/test_actions.py index e2028e3..b658181 100644 --- a/tests/test_actions.py +++ b/tests/test_actions.py @@ -5,13 +5,14 @@ import threading import time import unittest +from unittest import mock import _sdkpath # noqa: F401 -from litegrip import (CommandError, GraspResult, GripperConfig, GripperState, - LiteGrip, MotionConfig, ReleaseResult, UnitConversion, - clamp_to_travel, closing_travel_excursion_rad, - force_approach_terms, limit_target, press_target, - work_limit_target) +from litegrip import (CommError, CommandError, GraspResult, GripperConfig, + GripperState, LiteGrip, MotionConfig, ReleaseResult, + UnitConversion, clamp_to_travel, + closing_travel_excursion_rad, force_approach_terms, + limit_target, press_target, work_limit_target) from litegrip.actions import GripperActions, MoveResult from fake_can import (DT, POS_CLOSED_RAD, POS_OPEN_RAD, RAD_TO_MM, @@ -989,6 +990,73 @@ def test_neither_reached_nor_stalled_is_not_handed_over(self): self.assertTrue(any(f.kp == 0.0 for f in fake.frames), "没有撤力帧") +class TestForceApproachNeedsATrueStop(unittest.TestCase): + """2026-11-18 台架:5 N 接近段实际只有约 8 mm/s,0.25 s 走 2 mm。 + + 20% 的窗口阈值把「慢」判成「顶上东西」,保力段随后在自由行程里把夹爪 + 加速到 7 rad/s,从 78 mm 冲到 26 mm 的工件上。交接前必须看到**真停**: + 窗口平均速度低于 ``MotionConfig.stall_speed_mm_s``。 + """ + + def test_a_slow_creep_is_not_handed_over_mid_travel(self): + object_rad = (POS_OPEN_RAD + POS_CLOSED_RAD) / 2.0 + motion = MotionConfig(sleep_fn=lambda _: None, + monotonic_fn=tick_clock(0.1)) + crawl_rad_s = 0.109 # ≈ 8 mm/s at RAD_TO_MM + g, fake = make_gripper(block_rad=object_rad, stops=True, + motion=motion, speed_cap_rad_s=crawl_rad_s) + + g.grasp(force_n=5.0, hold_s=0.2) + + holds = [f for f in fake.frames if f.tau_ff != 0.0] + self.assertTrue(holds, "没有进入保力段") + first = holds[0].pos_after + self.assertLess( + abs(first - object_rad), g.motion_config.reach_tol, + "保力段在离工件 %.2f mm 处接手了 —— 又是自由行程里交接" % + (abs(first - object_rad) * RAD_TO_MM)) + + def test_a_slow_creep_without_contact_never_enters_the_hold(self): + """没有工件、只是被摩擦压在 8 mm/s:走到帧数上界按拒夹收尾。 + + 这条钉的是「不交接」本身,而不只是「交接点提前」:慢速蠕动既没到位 + 也没真停,保力段一帧都不许发 —— 它是纯力矩源,接过去只会继续加速。 + """ + motion = MotionConfig(sleep_fn=lambda _: None, + monotonic_fn=tick_clock(0.1)) + g, fake = make_gripper(stops=True, motion=motion, + speed_cap_rad_s=0.109) # ≈ 8 mm/s + + res = g.grasp(force_n=5.0, hold_s=0.2) + + self.assertFalse(res.ok, res) + self.assertFalse(res.stalled, res) + self.assertFalse(res.reached, res) + self.assertEqual(res.cycles, 0) + self.assertEqual([f for f in fake.frames if f.tau_ff != 0.0], [], + "慢速蠕动被交给了纯力矩的保力段") + + +class TestMotionRefreshesAnIdleBus(unittest.TestCase): + """空闲总线上的读数只在状态帧里更新;动作不能从手扳之前的缓存起跳。""" + + def test_open_does_not_start_from_a_position_read_before_the_bus_went_idle(self): + g, fake = make_gripper(stops=True, freeze_position_while_idle=True) + g.get_state() # 缓存停在张开侧 + fake.delivers = False + actual = POS_CLOSED_RAD - 0.4 # 总线空闲时被手扳到中间 + fake.motor.pos = actual + + g.open() + + first = fake.frames[0] + self.assertLess( + abs(first.q - actual), 0.01, + "open 从缓存位置起跳 %.1f mm(实际在 %.1f mm)" % + (abs(first.q - actual) * RAD_TO_MM, + abs(actual - fake.motor.pos) * RAD_TO_MM)) + + class TestRelease(unittest.TestCase): """松开:跨线程结束 ``grasp()``、从交接点张余量、急停、不重复张开。 @@ -2163,5 +2231,81 @@ def test_a_calibrated_but_zero_travel_config_is_refused_too(self): self.assertEqual(fake.frames, []) +class TestFreshStateRefusesAnUnansweredCache(unittest.TestCase): + """缓存还新不等于位置可信:0xCC 没人答,就不许规划运动。""" + + def test_a_recent_cache_is_not_enough_when_nothing_answers(self): + g, fake = make_gripper(stops=True) + g.get_state() # 先让缓存里有一帧、age 很小 + fake.delivers = False # 总线安静 + fake.refresh_ok = False # 0xCC 也发不出去 + + with self.assertRaises(CommError): + g.open() + + +class TestAbortReleaseNeedsNoFreshFrame(unittest.TestCase): + """急停只发一帧 kp=kd=tau=0;电机不答状态帧也必须撤力。""" + + def test_abort_releases_when_the_motor_does_not_answer(self): + g, fake = make_gripper(stops=True) + g.get_state() # 缓存里有一帧 + fake.delivers = False # 电机不主动开口 + fake.refresh_status = lambda: True # 0xCC 写成功,但电机不答 + + res = g.release(abort=True) + + self.assertFalse(res.ok) + self.assertTrue( + any(f.kp == 0.0 and f.kd == 0.0 and f.tau_ff == 0.0 + for f in fake.frames), + "急停没有发出零力矩帧 —— 最后一档力矩留在了总线上") + + +class TestFinalReachUsesAFreshFrame(unittest.TestCase): + """移动结束判 reached/ok 前必须再要一帧,不能拿循环里的缓存。""" + + def test_the_end_of_a_move_requests_a_status_frame(self): + g, fake = make_gripper(stops=True) + + res = g.open(SPEED_MM_S) + + self.assertTrue(res.ok, res) + self.assertGreaterEqual( + fake.refresh_calls, 2, + "移动结束时没有重新要状态帧 —— reached/ok 用的是缓存") + + +class TestGraspHoldCommError(unittest.TestCase): + """保力中途断总线:撤力、返回 ok=False,不把异常抛给调用方。""" + + def test_a_mid_hold_comm_error_releases_and_reports_failure(self): + block = (POS_OPEN_RAD + POS_CLOSED_RAD) / 2.0 + g, fake = make_gripper(block_rad=block) + + with mock.patch.object(g._actions, "_hold_force", + side_effect=CommError("bus down")): + res = g.grasp(force_n=5.0, hold_s=0.2) + + self.assertFalse(res.ok, res) + self.assertEqual(res.cycles, 0) + self.assertTrue(res.stalled, "接近段应当先夹到工件") + self.assertTrue( + any(f.kp == 0.0 and f.kd == 0.0 and f.tau_ff == 0.0 + for f in fake.frames), + "保力中断后没有撤力帧 —— 最后一档力矩留在了总线上") + self.assertTrue(g._actions._hold_done.is_set(), + "grasp() 的 finally 没有收尾") + + +class TestMotionConfigValidation(unittest.TestCase): + + def test_non_finite_or_negative_stall_speed_is_rejected(self): + for value in (-1.0, float("nan"), float("inf")): + with self.subTest(value=value): + with self.assertRaises(ValueError): + MotionConfig(stall_speed_mm_s=value) + + if __name__ == "__main__": unittest.main() diff --git a/tests/test_can_poll_drain.py b/tests/test_can_poll_drain.py new file mode 100644 index 0000000..995bf7a --- /dev/null +++ b/tests/test_can_poll_drain.py @@ -0,0 +1,151 @@ +"""``MotorController.poll`` must drain the RX queue; the newest frame wins. + +The motor answers every control frame with a status frame. A caller that +sends a burst (a force-hold slice, a ramp step) and polls once per burst leaves +the answers queued; reading one frame returns an old position, and the age is +stamped when it is read, so the stale reading looks fresh. On 2026-11-18 the +ramp after a hold started from the hold's first pose (78 mm) while the jaws +were at 26 mm — the first frame demanded ~0.7 rad, and the drive left at +23 rad/s out of a 20 mm/s move. +""" + +from __future__ import annotations + +import unittest + +import _sdkpath # noqa: F401 +from litegrip.can.controller import MAX_DRAIN_FRAMES, MotorController +from litegrip.can.protocol import float_to_uint +from litegrip.can.transport import CanFrame + +MST_ID = 0x18 +CAN_ID = 0x08 + + +def status_frame(position_rad: float, err: int = 1, + can_id: int = CAN_ID, mst_id: int = MST_ID) -> CanFrame: + """Build one DM status frame (q, dq=0, tau=0).""" + q_uint = float_to_uint(position_rad, -12.5, 12.5, 16) + dq_uint = float_to_uint(0.0, -30.0, 30.0, 12) + tau_uint = float_to_uint(0.0, -10.0, 10.0, 12) + data = bytes([ + ((err & 0x0F) << 4) | (can_id & 0x0F), + (q_uint >> 8) & 0xFF, + q_uint & 0xFF, + (dq_uint >> 4) & 0xFF, + ((dq_uint & 0x0F) << 4) | ((tau_uint >> 8) & 0x0F), + tau_uint & 0xFF, + 30, 30, + ]) + return CanFrame(can_id=mst_id, data=data) + + +class QueueTransport: + """A transport that hands out pre-queued frames, then goes quiet.""" + + def __init__(self, frames): + self._frames = list(frames) + self.sent = [] + + def recv(self, timeout_s: float = 0.0): + return self._frames.pop(0) if self._frames else None + + def send(self, can_id, data, timeout=0.1): + self.sent.append((can_id, data)) + + def send_multi(self, can_id, data, count=1, interval_s=0.002): + for _ in range(count): + self.send(can_id, data) + + def set_id_filter(self, ids): + pass + + +class TestPollDrainsTheQueue(unittest.TestCase): + + def make_controller(self, frames): + transport = QueueTransport(frames) + controller = MotorController(transport) + motor = controller.add_motor(can_id=CAN_ID, mst_id=MST_ID) + return controller, motor + + def test_the_newest_queued_frame_wins(self): + controller, motor = self.make_controller([ + status_frame(-1.0), + status_frame(-0.5), + status_frame(-0.2), + ]) + + updated = controller.poll() + + self.assertIs(updated, motor) + self.assertEqual(motor.rx_count, 3, "队列没有被排空") + self.assertAlmostEqual(motor.position, -0.2, places=3, + msg="读到的还是队列里最旧的那一帧") + + def test_foreign_frames_between_status_frames_do_not_stop_the_drain(self): + foreign = CanFrame(can_id=0x123, data=bytes(8)) + controller, motor = self.make_controller([ + status_frame(-1.0), + foreign, + status_frame(-0.3), + ]) + + controller.poll() + + self.assertEqual(motor.rx_count, 2) + self.assertAlmostEqual(motor.position, -0.3, places=3) + + def test_a_single_frame_is_still_reported(self): + controller, motor = self.make_controller([status_frame(0.25)]) + + updated = controller.poll() + + self.assertIs(updated, motor) + self.assertEqual(motor.rx_count, 1) + self.assertAlmostEqual(motor.position, 0.25, places=3) + + def test_an_empty_queue_returns_none(self): + controller, _ = self.make_controller([]) + + self.assertIsNone(controller.poll()) + + +class TestPollUntilSeesTheTargetUpdate(unittest.TestCase): + """``poll()`` reports only the newest motor; ``poll_until`` must not.""" + + def test_the_target_counts_even_when_another_motor_is_newest(self): + transport = QueueTransport([ + status_frame(-1.0, can_id=0x08, mst_id=0x18), + status_frame(-0.5, can_id=0x09, mst_id=0x19), + ]) + controller = MotorController(transport) + target = controller.add_motor(can_id=0x08, mst_id=0x18) + controller.add_motor(can_id=0x09, mst_id=0x19) + + self.assertTrue(controller.poll_until(target, timeout_s=0.05), + "目标电机的帧先到、另一个电机最后被排空,更新丢了") + self.assertEqual(target.rx_count, 1) + + +class TestDrainIsBounded(unittest.TestCase): + """A bus that never goes quiet must not hold ``poll`` indefinitely.""" + + def test_the_drain_stops_at_the_cap_and_leaves_the_rest_queued(self): + transport = QueueTransport( + [status_frame(0.0) for _ in range(MAX_DRAIN_FRAMES + 5)]) + controller = MotorController(transport) + motor = controller.add_motor(can_id=CAN_ID, mst_id=MST_ID) + + controller.poll() + + self.assertEqual(motor.rx_count, MAX_DRAIN_FRAMES) + + controller.poll() + + self.assertEqual(motor.rx_count, MAX_DRAIN_FRAMES + 5, + "未排空的帧没有留给下一次 poll") + + +if __name__ == "__main__": + unittest.main()