From 8ed5cc3ce53616cce2d1b5bd750513af42f42773 Mon Sep 17 00:00:00 2001 From: cao-xiao-hao <127832643+cao-xiao-hao@users.noreply.github.com> Date: Fri, 30 Oct 2026 14:06:58 +0800 Subject: [PATCH 1/2] fix: refuse the handover when the approach never moved A force-carrying approach that stops within `no_bite_mm` (0.5 mm) of where it started was being reported as `stalled=True` and handed over to the pure-torque hold. That is not contact: at 2 N the approach budget is 0.18 N*m, below the measured 1.8-2.0 N breakaway, so the jaws never leave static friction. The hold then applies 0.20 N*m with no position or velocity term, and across a free stroke that torque accelerates the jaws into the runaway watch -- the "fast close" seen on the bench. The stall verdict now compares the stopped pose with the pose the approach began at. Inside `no_bite_mm` the move is flagged `no_bite` instead of `stalled`: `grasp()` releases in place, sends no hold frame and returns `ok=False, no_bite=True, cycles=0`. Real contact delta above the threshold is unchanged, as is the empty-jaw `reached` path. The fake motor gained `breakaway_tau_nm` so the regression is reproducible in simulation. --- README.md | 15 ++++++-- README.zh-CN.md | 14 ++++++-- src/litegrip/actions.py | 78 ++++++++++++++++++++++++++++++++++++++--- tests/fake_can.py | 27 +++++++++++--- tests/test_actions.py | 67 +++++++++++++++++++++++++++++++++++ 5 files changed, 187 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index a32ff11..1910bc8 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). `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). A stop that came **without the jaw ever moving from where the approach began** is not treated as contact — the budget may simply be below 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. | @@ -359,8 +359,8 @@ What it *means*, and what to check, is per action: | Type | Fields | `bool()` is | | --- | --- | --- | -| `MoveResult` | `ok`, `reached`, `stalled`, `state`, `target_rad`, `limit_rad`, `final_cmd_rad`, `steps`, `protected`, `released` | `ok` | -| `GraspResult` | `ok`, `reached`, `stalled`, `state`, `target_rad`, `force_n`, `cycles`, `travel_guard`, `released` | `ok` | +| `MoveResult` | `ok`, `reached`, `stalled`, `state`, `target_rad`, `limit_rad`, `final_cmd_rad`, `steps`, `protected`, `released`, `no_bite` | `ok` | +| `GraspResult` | `ok`, `reached`, `stalled`, `state`, `target_rad`, `force_n`, `cycles`, `travel_guard`, `released`, `no_bite` | `ok` | | `ReleaseResult` | `ok`, `state`, `opened_mm`, `target_rad`, `aborted` | `ok` | | `EnableResult` | `ok`, `state`, `tries` | `ok` | @@ -386,6 +386,14 @@ A fourth combination is a failure, and new in this version: `cycles=0`. Nothing was gripped. If you see this, the closing leg is being starved — a stale state read, or friction the mechanism's own budget cannot overcome. +- **`grasp()` with `ok=False, no_bite=True`** means the approach carried its force but never moved + the jaws: it stopped within `MotionConfig.no_bite_mm` (0.5 mm) of where it started. That is not + contact — it is more often a budget that cannot break static friction than a part sitting at the + start pose — and it is exactly the case where handing over the hold would send a jaw with a free + stroke ahead of it into the torque-only frames and let it accelerate. So the hold is never + entered, the call releases in place, and `cycles` is 0. `stalled` and `reached` are both `False` + on this path. The remedy is a setpoint above the breakaway, not a longer hold. + - **`grasp()` ended by a release** returns `released=True`: `ok=True` when the opening completed, `ok=False` when it was an E-stop (`release(abort=True)`), which sends no opening move. `reached` and `stalled` still describe the closing leg; `state` is the pose after the release. @@ -447,6 +455,7 @@ with LiteGrip("can0") as gripper: | `max_lead_mm` | `4.0` | cap on how far the command may lead the measured position, on moves that carry no force setpoint | | `stop_lead_mm` | `0.7` | the same cap on the approach to a stop, so the pressing torque stays bounded | | `press_safety` | `0.9` | fraction of the setpoint a force-carrying approach may spend | +| `no_bite_mm` | `0.5` | a force-carrying approach that stops this close to its start pose is refused, not read as contact (see `grasp()` above) | | `reach_tol` | `0.02` | position tolerance for `reached`, radians | | `stop_tol` | `0.03` | how close to the calibrated stop the jaw must park to count as pressed home | | `settle_s` | `0.3` | time spent holding the target after the ramp | diff --git a/README.zh-CN.md b/README.zh-CN.md index aae8a6e..76dcf91 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`(空爪闭到底)。`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`(空爪闭到底)。但**从起点就没动过**就停下不算接触 —— 也可能是预算低于静摩擦 —— 这种情形调用原地撤力、返回 `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` 的默认值更长。 | @@ -309,8 +309,8 @@ print(abs(cfg.pos_open_rad - cfg.pos_closed_rad) * cfg.rad_to_mm) # 行程,m | 类型 | 字段 | `bool()` 是 | | --- | --- | --- | -| `MoveResult` | `ok`、`reached`、`stalled`、`state`、`target_rad`、`limit_rad`、`final_cmd_rad`、`steps`、`protected`、`released` | `ok` | -| `GraspResult` | `ok`、`reached`、`stalled`、`state`、`target_rad`、`force_n`、`cycles`、`travel_guard` | `ok` | +| `MoveResult` | `ok`、`reached`、`stalled`、`state`、`target_rad`、`limit_rad`、`final_cmd_rad`、`steps`、`protected`、`released`、`no_bite` | `ok` | +| `GraspResult` | `ok`、`reached`、`stalled`、`state`、`target_rad`、`force_n`、`cycles`、`travel_guard`、`released`、`no_bite` | `ok` | | `EnableResult` | `ok`、`state`、`tries` | `ok` | **看 `ok`。** 下面三种组合看着吓人,其实都是对的: @@ -332,6 +332,13 @@ print(abs(cfg.pos_open_rad - cfg.pos_closed_rad) * cfg.rad_to_mm) # 行程,m 撤力。此时调用改为原地撤力,报 `cycles=0`。什么也没夹住。见到它就说明闭合段被饿着了 —— 读数过期,或者机构自身预算吃不掉的摩擦。 +- **`grasp()` 返回 `ok=False, no_bite=True`**,意思是接近段带着力,却**从起点就没推动过** + 爪子:停住时离起始位置还不到 `MotionConfig.no_bite_mm`(0.5 mm)。这不是接触 —— 更常见的 + 是力矩预算压不出脱离静摩擦需要的力,而不是工件正好摆在起点 —— 而这恰好就是交接会让爪子 + 带着一整段自由行程冲进纯力矩帧、一路加速的情形。所以保力段一帧都不发,调用原地撤力, + `cycles` 为 0,`stalled` 与 `reached` 在这条路上都是 `False`。要救它得把设定值抬到静摩擦 + 之上,不是把保力拉长。 + 在离限位很远的地方堵转,才会给 `ok=False`:有东西挡住了行程。为假时该记的,就是那几个诊断 字段 —— `final_cmd_rad`、`steps`、`state`。 @@ -386,6 +393,7 @@ with LiteGrip("can0") as gripper: | `max_lead_mm` | `4.0` | 不带力设定值的运动里,指令允许领先实测位置的上限 | | `stop_lead_mm` | `0.7` | 接近限位时的同一上限,用来压住压紧力矩 | | `press_safety` | `0.9` | 带力的接近段最多能花掉设定值的多少 | +| `no_bite_mm` | `0.5` | 带力接近段离起点这么近就停住时,判为没咬住、不交接(见上文 `grasp()`) | | `reach_tol` | `0.02` | 判定 `reached` 的位置容差,rad | | `stop_tol` | `0.03` | 要算「压到位」,爪子得停到离标定限位多近 | | `settle_s` | `0.3` | 爬坡结束后在目标上停留的时间 | diff --git a/src/litegrip/actions.py b/src/litegrip/actions.py index d62b007..84fa541 100644 --- a/src/litegrip/actions.py +++ b/src/litegrip/actions.py @@ -151,6 +151,23 @@ class MotionConfig: # 照设定的力往下发。 press_safety: float = 0.9 # 预算余量:撞上时实测力落在设定值之下 + # ── 带设定力的接近段「从起点就没动过」的判据 mm ─────────────────── + # 位置窗口判据只能看见「没动」,可「没动」有三种:真顶到工件、顶到限位、 + # 以及**这一段的力矩预算低于静摩擦,压根没能脱离**。第三种最危险:它会被 + # 当成接触,交接给纯力矩的保力段 —— 那一段没有位置项也没有速度项,而保力 + # 的力矩是设定值的 1.0 倍、接近段只有 0.9 倍,多出来的这一成正好越过脱离 + # 力,于是夹爪带着设定力在自由行程里一路加速(2026-10-29 实测:2 N 夹取 + # 的接近段压 0.18 Nm 原地不动,接手的保力段压 0.20 Nm 推了 19 mm)。 + # + # 所以带设定力的接近段里,位移小于这个值的停住**不算接触**:报「没能脱离 + # 静摩擦」(``MoveResult.no_bite``)、撤力、不交接。代价是「工件就贴在起点 + # 旁边、夹爪还没走开就顶上了」也按这个处理 —— 那一次夹取会报失败,而反过来 + # 放行会把一次误判变成一次带力冲程,两边代价不对称,所以选失败。 + # + # 0.5 mm 是台架闭合侧粘滑死区(约 0.0103 rad ≈ 0.76 mm)的下面:比死区更短 + # 的「顶住」读数本来就分辨不出来,真工件也做不到停在这么窄的缝里。 + no_bite_mm: float = 0.5 + # ── 行进段堵转保护(≈7 N) ──────────────────────────────────────── # 普通移动(``press=True`` 的 open/close)行进段的领先上限是 max_lead_mm, # 一旦夹爪在半路被硬挡,``kp × 领先上限`` ≈ 7.6 N·m 会一直压着;而位置 @@ -276,6 +293,11 @@ class MoveResult: # 只有 ``grasp()`` 的带力接近段会检查它:那一段走的不是用户要的终点,而是 # 夹取的第一步,松开请求意味着「别夹了」。``grasp()`` 据此改走张开。 release_requested: bool = False + # 带设定力的接近段**从起点就没动过**:停住时离起始位置还不到 + # :attr:`MotionConfig.no_bite_mm`。这不是接触 —— 也可能是力矩预算低于静 + # 摩擦、夹爪压根没能脱离(判据和代价见 ``MotionConfig.no_bite_mm``)。置位 + # 时 ``stalled`` 为 False,``grasp()`` 不交接、直接撤力返回。 + no_bite: bool = False def __bool__(self) -> bool: return self.ok @@ -302,6 +324,11 @@ class GraspResult: # 请求后先完成松开(张开到 ``max(当前, 夹取点) + 余量``),再返回。与 # ``travel_guard`` 的区别是这不是故障 —— 松开是用户要的正常收尾。 released: bool = False + # 接近段从起点就没能推动夹爪:位移小于 ``MotionConfig.no_bite_mm``,设定值 + # 的 0.9 倍力矩压不出脱离静摩擦需要的力。这一条**不交接**,保力段一帧都不 + # 发 —— 交过去它会在自由行程里一路加速(见 ``MotionConfig.no_bite_mm``)。 + # 置位时 ``stalled`` / ``reached`` 都是 False,``cycles`` 为 0。 + no_bite: bool = False def __bool__(self) -> bool: return self.ok @@ -854,6 +881,30 @@ def grasp( if req is not None: return self._grasp_ends_with_release(req, move, force, cycles=0) + if move.no_bite: + # 接近段从起点就没动过:可能是工件就在那里,也可能是设定值的 + # 0.9 倍力矩压不出脱离静摩擦需要的力。两者在电机看来一模一样, + # 而接手的是纯力矩的保力段(没有位置、没有限速,力矩还比接近段 + # 高一成)—— 猜错的代价是一次带力冲程,猜对(拒夹)的代价只是 + # 一次报失败。所以宁可报失败,也不交接。 + self._release(move.state.position_rad) + log.warning( + "夹取未交接:接近段从起点就没能推动夹爪(设定 %.1f N,预算 " + "%.3f Nm,停在 %.1f mm,目标是闭合端)。保力段是纯力矩源," + "交过去会在自由行程里一路加速 —— 撤力返回,不下发保力。", + force, force * UnitConversion.N_TO_NM * cfg.press_safety, + move.state.position_mm) + return GraspResult( + ok=False, + reached=move.reached, + stalled=move.stalled, + state=move.state, + target_rad=move.target_rad, + force_n=force, + cycles=0, + no_bite=True, + ) + if not (move.reached or move.stalled): # 既没到位也没顶上东西 —— 夹爪还在自由行程里,而位置环是此刻 # 唯一拦着它的东西(kp × 领先 + kd × 速度误差)。保力段的帧里 @@ -1558,6 +1609,7 @@ def _move_to_limit( hist = [before.position_rad] stalled = False + no_bite = False # 带力接近段从起点就没动过(见上) protected = False release_requested = False # 松开请求叫停了带力接近段 over_torque = 0 # 连续过阈的采样点数 @@ -1653,10 +1705,27 @@ def _move_to_limit( if (judge_stall and win_delta is not None and (press or in_ramp) and win_delta < win_thresh_rad): - stalled = True - log.info("堵转:最近 %d 次采样(%.2f s)净位移仅 %.5f rad " - "(< %.5f),停在 %+.5f rad", - win, win_s, win_delta, win_thresh_rad, pos) + # 「没动」不等于「顶到工件」:带设定力时,从起点起就没挪过位的 + # 停住也可能是这一段压不出脱离静摩擦的力矩。这一条不能当接触, + # 否则交接给纯力矩的保力段就是一次带力冲程(见 + # ``MotionConfig.no_bite_mm``)。 + if (force_n is not None + and abs(pos - before.position_rad) + < cfg.no_bite_mm / gcfg.rad_to_mm): + no_bite = True + log.warning( + "接近段没能脱离静摩擦:预算 %.3f Nm 下,夹爪从 %+.5f rad " + "起只挪了 %.2f mm 就停住(目标 %+.5f rad,还差 %.1f mm)。" + "「没动」也可能是力矩不够,不是顶到工件 —— 不交接、撤力。", + force_n * UnitConversion.N_TO_NM * cfg.press_safety, + before.position_rad, + abs(pos - before.position_rad) * gcfg.rad_to_mm, + target, abs(pos - target) * gcfg.rad_to_mm) + else: + stalled = True + log.info("堵转:最近 %d 次采样(%.2f s)净位移仅 %.5f rad " + "(< %.5f),停在 %+.5f rad", + win, win_s, win_delta, win_thresh_rad, pos) break # 行进段堵转力矩保护(≈7 N):与位置窗口判据**互补** —— 位置窗口对 @@ -1703,6 +1772,7 @@ def _move_to_limit( steps=last_i, protected=protected, release_requested=release_requested, + no_bite=no_bite, ) def _hold_force( diff --git a/tests/fake_can.py b/tests/fake_can.py index 7d71c18..bdb3d03 100644 --- a/tests/fake_can.py +++ b/tests/fake_can.py @@ -81,7 +81,11 @@ def __init__(self, pos: float = 0.0, block_rad: Optional[float] = None, limit_hi: 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): + report_lag_ticks: int = 0, + breakaway_tau_nm: float = 0.0): + # 静摩擦的脱离力矩 Nm:站在原地的夹爪要帧里力矩过这个门槛才动。 + # 0 = 没有静摩擦(默认,别的用例逐位不变)。 + self.breakaway_tau_nm = breakaway_tau_nm # 读数延迟(帧)。真机上 ``get_state(wait=False)`` 拿的是缓存读数,可能 # 落后几帧;而力控接近段的指令是 ``min(排程, 读数 + 领先上限)``,领先只 # 有零点几毫米时,``速度 × 延迟`` 就把它吃光了 —— 夹爪于是推不动排程。 @@ -155,6 +159,13 @@ def step(self, q: float, kp: float, dq: float, tau_ff: float, dt: float) -> None v = dq + self.GAIN * (kp / self.KP_NOMINAL) * (q - self.pos) # 纯前馈力矩下的蠕变(见类上方的说明)。默认 0 时不参与运算。 v += self.creep_rad_s_per_nm * tau_ff + # 静摩擦:**站在原地的**夹爪,帧里的力矩(``kp × 位移 + 前馈``)不过 + # ``breakaway_tau_nm`` 就一动不动;一旦动起来这道门就不再拦(动摩擦 + # 按 0 建),所以是「脱离力」而不是「阻力」。真机上 0.9 倍设定值的 + # 接近段压不出脱离力、夹爪原地站住,就是这一个模型。 + 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)) new = self.pos + v * dt if self.block_rad is not None: @@ -194,14 +205,16 @@ def __init__(self, pos: float = 0.0, block_rad: Optional[float] = None, limit_hi: 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): + report_lag_ticks: int = 0, + breakaway_tau_nm: float = 0.0): self.motor = FakeMotor(pos=pos, block_rad=block_rad, sticky_rad=sticky_rad, err=err, limit_lo=limit_lo, limit_hi=limit_hi, yield_rad_s=yield_rad_s, yield_tau_nm=yield_tau_nm, creep_rad_s_per_nm=creep_rad_s_per_nm, - report_lag_ticks=report_lag_ticks) + report_lag_ticks=report_lag_ticks, + breakaway_tau_nm=breakaway_tau_nm) self.frames: List[Frame] = [] self.initialize_results = list(initialize_results or []) self.initialize_calls = 0 @@ -370,6 +383,7 @@ def make_gripper( 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, ): """造一个「已连接、已使能、已标定」的 LiteGrip,底层换成 :class:`FakeLiteGripCAN`。 @@ -394,6 +408,10 @@ def make_gripper( 拿的是缓存读数。力控接近段的指令是 ``min(排程, 读数 + 领先上限)``,领先只有 零点几毫米时延迟就把它吃光了,夹爪因此推不动排程。默认 0,不影响别的用例。 + ``breakaway_tau_nm`` 给夹爪装上静摩擦:站在原地的夹爪要帧里力矩过这个门槛 + 才动,动起来以后不再拦。用来复现「接近段的力矩预算低于脱离力,夹爪从起点就 + 没动过」那个工况(判据见 ``MotionConfig.no_bite_mm``)。默认 0 = 无静摩擦。 + ``start_rad`` 默认停在张开侧(两种装法各自的那一端)。 Returns: @@ -419,7 +437,8 @@ def make_gripper( yield_rad_s=yield_rad_s, yield_tau_nm=yield_tau_nm, creep_rad_s_per_nm=creep_rad_s_per_nm, - report_lag_ticks=report_lag_ticks) + report_lag_ticks=report_lag_ticks, + breakaway_tau_nm=breakaway_tau_nm) g._can = fake g._connected = True g._enabled = True diff --git a/tests/test_actions.py b/tests/test_actions.py index c05fa36..d04bc53 100644 --- a/tests/test_actions.py +++ b/tests/test_actions.py @@ -1879,6 +1879,73 @@ def test_the_hold_still_lands_on_the_setpoint(self): self.assertAlmostEqual(f.tau_ff, 2.0, places=9) +class TestTheApproachRefusesToHandOverWithoutBite(unittest.TestCase): + """接近段从起点就没动过时不许交接:那是「没能脱离静摩擦」,不是接触。 + + 2026-10-29 台架实测(2 N 夹取):接近段压 0.18 Nm,整个 0.25 s 堵转窗口只挪了 + 0.0004 rad;它被判成「顶到工件」,接手的保力段压 0.20 Nm —— 设定值的 1.0 倍, + 比接近段高出一成,正好越过脱离力 —— 于是夹爪带着设定力在自由行程里推了 19 mm, + 夹取点报在工件前 19–39 mm。操作者看到的是「先不动,然后飞速闭合」。 + + 判据见 ``MotionConfig.no_bite_mm``:带设定力的接近段里,位移小于这个值的停住 + 不算接触 —— 报 ``no_bite``、撤力,保力段一帧都不发。 + """ + + #: 脱离力:2 N 的预算(0.18 Nm)压不动,3 N 的(0.27 Nm)压得动。 + BREAKAWAY_NM = 0.22 + + def _grasp(self, force_n, hold_s=0.2, **kw): + motion = MotionConfig(sleep_fn=lambda _: None, + monotonic_fn=tick_clock(0.1)) + g, fake = make_gripper(stops=True, motion=motion, + breakaway_tau_nm=self.BREAKAWAY_NM, **kw) + return g, fake, g.grasp(force_n=force_n, hold_s=hold_s) + + @staticmethod + def _budget(force_n): + return force_n * UnitConversion.N_TO_NM * MotionConfig().press_safety + + def test_the_setpoint_below_the_breakaway_is_refused_not_held(self): + _g, fake, res = self._grasp(2.0) + + self.assertLess(self._budget(2.0), self.BREAKAWAY_NM) # 前提 + self.assertFalse(res.ok, res) + self.assertTrue(res.no_bite, 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], [], + "保力段一帧都不许发:它是纯力矩源,没有位置也没有限速") + self.assertEqual(fake.frames[-1].kp, 0.0, "结尾要撤力") + + def test_the_setpoint_above_the_breakaway_still_grasps(self): + _g, fake, res = self._grasp(3.0) + + self.assertGreater(self._budget(3.0), self.BREAKAWAY_NM) # 前提 + self.assertFalse(res.no_bite, res) + self.assertTrue(res.reached, res) + self.assertTrue(res.ok, res) + self.assertGreater(res.cycles, 0) + + def test_a_workpiece_at_the_start_is_refused_as_well(self): + """代价写在测试里:工件就贴在起点时,这一次夹取也会被拒。 + + 电机分不出「顶住工件」和「压不出脱离力」—— 两者都是零位移。拒夹的代价 + 是操作者看到一次失败、重摆一次;放行的代价是一次带力冲程(上面那个 19 mm + 的推程)。两边不对称,所以选拒夹。 + """ + start = POS_OPEN_RAD + block = start + 0.001 * (POS_CLOSED_RAD - POS_OPEN_RAD) # ≈ 0.09 mm + motion = MotionConfig(sleep_fn=lambda _: None, + monotonic_fn=tick_clock(0.1)) + g, fake = make_gripper(stops=True, motion=motion, block_rad=block) + res = g.grasp(force_n=20.0, hold_s=0.2) + + self.assertTrue(res.no_bite, res) + self.assertFalse(res.ok, res) + self.assertEqual([f for f in fake.frames if f.tau_ff != 0.0], []) + + class TestNonFiniteInputIsRefused(unittest.TestCase): """非有限值不许变成运动:拒绝带原因,而且**一帧都不发**。 From 40252ee69180fe7e1c66e613a95a45c153a1e632 Mon Sep 17 00:00:00 2001 From: cao-xiao-hao <127832643+cao-xiao-hao@users.noreply.github.com> Date: Fri, 30 Oct 2026 15:43:48 +0800 Subject: [PATCH 2/2] fix: gate the no-bite refusal on the breakaway budget MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Review feedback (X-F-R): the refusal applied at every setpoint, so a part already against the open jaw — or a re-grasp in place after release(abort=True), which sends no opening move — was refused even at 20 N, where a zero-displacement stop is contact, not a budget that cannot break away. MotionConfig.breakaway_nm (0.2 Nm, the bracket measured on 2026-10-29) now gates the verdict: only a budget below it turns a zero-displacement stop into no_bite. Above it the stop stays `stalled` and is held, which is the setpoint the caller asked for with nothing ahead of the jaws to run into. The regression test now reproduces the runaway it is named after: a fake with breakaway 0.19 Nm and creep, run with no_bite_mm=0 (the pre-fix behaviour), walks the jaws >10 mm under the hold; the same bench with the refusal on does not move them at all. examples/grasp_test.py reports no_bite as the refusal it is instead of "the closing leg ran out of frames" or "err=0". Both refusal paths share one result constructor, the grasp docstrings name the new field, and the READMEs call it the fifth combination, state the gate, and note the reach_tol edge. --- README.md | 22 +++++++--- README.zh-CN.md | 22 +++++++--- examples/grasp_test.py | 38 ++++++++++++++++- src/litegrip/actions.py | 93 +++++++++++++++++++++++++++++------------ src/litegrip/gripper.py | 7 +++- tests/test_actions.py | 79 ++++++++++++++++++++++++++++------ 6 files changed, 207 insertions(+), 54 deletions(-) diff --git a/README.md b/README.md index 1910bc8..0a66f93 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** is not treated as contact — the budget may simply be below 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). 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. | @@ -386,13 +386,24 @@ A fourth combination is a failure, and new in this version: `cycles=0`. Nothing was gripped. If you see this, the closing leg is being starved — a stale state read, or friction the mechanism's own budget cannot overcome. +A fifth is a refusal: + - **`grasp()` with `ok=False, no_bite=True`** means the approach carried its force but never moved - the jaws: it stopped within `MotionConfig.no_bite_mm` (0.5 mm) of where it started. That is not - contact — it is more often a budget that cannot break static friction than a part sitting at the - start pose — and it is exactly the case where handing over the hold would send a jaw with a free + the jaws: it stopped within `MotionConfig.no_bite_mm` (0.5 mm) of where it started *and* its + budget (`force_n × 0.1 × press_safety`) was below `MotionConfig.breakaway_nm` (0.2 N·m on this + unit, so setpoints up to 2.2 N). That is not contact — it is a budget that cannot break static + friction — and it is exactly the case where handing over the hold would send a jaw with a free stroke ahead of it into the torque-only frames and let it accelerate. So the hold is never entered, the call releases in place, and `cycles` is 0. `stalled` and `reached` are both `False` - on this path. The remedy is a setpoint above the breakaway, not a longer hold. + on this path. The remedy is a setpoint above the breakaway (~2.5 N here), not a longer hold. + Above the breakaway a zero-displacement stop *is* contact and is held normally: a part already + against the open jaw, or a re-grasp in place after `release(abort=True)`, still grips. + +Two edges worth knowing. An approach that starts within `reach_tol` (0.9 mm) of the closed stop +gets no stall verdict at all — it reports `reached` instead, and the hold has nothing to run +because the jaws are already on the end stop. And the refusal is about the budget, not the part: +if a grippy part sits at the start pose *and* the setpoint is below the breakaway, the grasp is +refused rather than risked. - **`grasp()` ended by a release** returns `released=True`: `ok=True` when the opening completed, `ok=False` when it was an E-stop (`release(abort=True)`), which sends no opening move. `reached` @@ -456,6 +467,7 @@ with LiteGrip("can0") as gripper: | `stop_lead_mm` | `0.7` | the same cap on the approach to a stop, so the pressing torque stays bounded | | `press_safety` | `0.9` | fraction of the setpoint a force-carrying approach may spend | | `no_bite_mm` | `0.5` | a force-carrying approach that stops this close to its start pose is refused, not read as contact (see `grasp()` above) | +| `breakaway_nm` | `0.2` | measured static-friction breakaway; only a budget below it makes a zero-displacement stop a refusal rather than contact | | `reach_tol` | `0.02` | position tolerance for `reached`, radians | | `stop_tol` | `0.03` | how close to the calibrated stop the jaw must park to count as pressed home | | `settle_s` | `0.3` | time spent holding the target after the ramp | diff --git a/README.zh-CN.md b/README.zh-CN.md index 76dcf91..f282917 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`(空爪闭到底)。但**从起点就没动过**就停下不算接触 —— 也可能是预算低于静摩擦 —— 这种情形调用原地撤力、返回 `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`(空爪闭到底)。但**从起点就没动过**就停下、且预算低于 `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` 的默认值更长。 | @@ -332,12 +332,21 @@ print(abs(cfg.pos_open_rad - cfg.pos_closed_rad) * cfg.rad_to_mm) # 行程,m 撤力。此时调用改为原地撤力,报 `cycles=0`。什么也没夹住。见到它就说明闭合段被饿着了 —— 读数过期,或者机构自身预算吃不掉的摩擦。 +第五种组合是拒夹: + - **`grasp()` 返回 `ok=False, no_bite=True`**,意思是接近段带着力,却**从起点就没推动过** - 爪子:停住时离起始位置还不到 `MotionConfig.no_bite_mm`(0.5 mm)。这不是接触 —— 更常见的 - 是力矩预算压不出脱离静摩擦需要的力,而不是工件正好摆在起点 —— 而这恰好就是交接会让爪子 - 带着一整段自由行程冲进纯力矩帧、一路加速的情形。所以保力段一帧都不发,调用原地撤力, - `cycles` 为 0,`stalled` 与 `reached` 在这条路上都是 `False`。要救它得把设定值抬到静摩擦 - 之上,不是把保力拉长。 + 爪子:停住时离起始位置还不到 `MotionConfig.no_bite_mm`(0.5 mm),**而且**这一段的预算 + (`force_n × 0.1 × press_safety`)低于 `MotionConfig.breakaway_nm`(这台机器 0.2 Nm, + 也就是设定值 2.2 N 及以下)。这不是接触 —— 是力矩预算压不出脱离静摩擦需要的力 —— 而这 + 恰好就是交接会让爪子带着一整段自由行程冲进纯力矩帧、一路加速的情形。所以保力段一帧都不 + 发,调用原地撤力,`cycles` 为 0,`stalled` 与 `reached` 在这条路上都是 `False`。要救它得 + 把设定值抬到脱离力之上(这台机器约 2.5 N),不是把保力拉长。预算在脱离力之上时,零位移 + **就是**顶到了东西,照常交接:工件已经贴在张开侧、或者 `release(abort=True)` 之后原地 + 重夹,都还能夹住。 + +两个边界值得知道。起点落在闭合端 `reach_tol`(0.9 mm)以内时整段不判堵转 —— 它报 +`reached`,保力段前面本来也没有自由行程。另外判的是预算、不是工件:粘手的工件正好摆在起点、 +而设定值又低于脱离力时,这一次夹取会被拒。 在离限位很远的地方堵转,才会给 `ok=False`:有东西挡住了行程。为假时该记的,就是那几个诊断 字段 —— `final_cmd_rad`、`steps`、`state`。 @@ -394,6 +403,7 @@ with LiteGrip("can0") as gripper: | `stop_lead_mm` | `0.7` | 接近限位时的同一上限,用来压住压紧力矩 | | `press_safety` | `0.9` | 带力的接近段最多能花掉设定值的多少 | | `no_bite_mm` | `0.5` | 带力接近段离起点这么近就停住时,判为没咬住、不交接(见上文 `grasp()`) | +| `breakaway_nm` | `0.2` | 实测静摩擦脱离力矩;只有预算低于它时,零位移停住才算拒夹而不是接触 | | `reach_tol` | `0.02` | 判定 `reached` 的位置容差,rad | | `stop_tol` | `0.03` | 要算「压到位」,爪子得停到离标定限位多近 | | `settle_s` | `0.3` | 爬坡结束后在目标上停留的时间 | diff --git a/examples/grasp_test.py b/examples/grasp_test.py index d4fed6b..0894cfb 100644 --- a/examples/grasp_test.py +++ b/examples/grasp_test.py @@ -274,7 +274,8 @@ def _report(result, trace: Trace, cfg, motion, label: str, grasped: bool) -> str st = result.state lines = [ f" ok={result.ok} reached={result.reached} stalled={result.stalled} " - f"runaway_watch={result.travel_guard} err={st.error_code}", + f"no_bite={result.no_bite} runaway_watch={result.travel_guard} " + f"err={st.error_code}", f" stopped at {st.position_mm:+.1f} mm ({st.position_rad:+.6f} rad), " f"empty target was {to_mm(cfg, result.target_rad):+.1f} mm, " f"off by {abs(st.position_rad - result.target_rad) * cfg.rad_to_mm:.2f} mm", @@ -329,14 +330,45 @@ def _hold_problem(result) -> str | None: "the closed stop by more than MotionConfig.hold_runaway_rad, so " "the torque was dropped — either nothing is stopping the closing " "direction, or the calibration no longer describes the travel") + if result.no_bite: + # Not a hold problem at all: grasp() refused the hand-over, so no hold + # ran and there was no fault. The judges name this + # (_no_bite_problem); here it must not be mistaken for "err=0". + return None if not result.ok: return f"the hold phase ended abnormally (err={result.state.error_code})" return None -def _judge_empty(result, trace: Trace, cfg, _motion) -> list[str]: +def _no_bite_problem(result, motion) -> str: + """The one ending that is a refusal rather than a motion, said plainly. + + ``grasp()`` dropped the torque because the approach never moved the jaws + and its budget was below the breakaway (``MotionConfig.breakaway_nm``). + Reporting that as "the closing leg ran out of frames while still + travelling" — the message the other endings get — points the operator the + opposite way, and "err=0" reads like a fault that did not happen. + """ + floor_n = motion.breakaway_nm / (N_TO_NM * motion.press_safety) + return (f"refused, not held (no_bite=True): the closing leg never moved the " + f"jaws. Its budget ({result.force_n:g} N = " + f"{result.force_n * N_TO_NM:g} Nm, of which the approach may spend " + f"{motion.press_safety:g}) is below the breakaway " + f"({motion.breakaway_nm:g} Nm), so grasp() dropped the torque " + f"instead of handing the jaws to the torque-only hold — that would " + f"have powered them across the free stroke. The jaws stopped at " + f"{result.state.position_mm:+.1f} mm. Raise --force above " + f"{floor_n + 0.1:.1f} N (or check whether something is already " + f"against the jaws before closing)") + + +def _judge_empty(result, trace: Trace, cfg, motion) -> list[str]: """Clear jaws: it should reach the closed stop and not report a stall.""" problems = [] + if result.no_bite: + # A refusal, not a movement. Everything below judges the closing leg + # and the hold that followed — neither ran. + return [_no_bite_problem(result, motion)] if not result.reached: short_mm = (abs(result.state.position_rad - result.target_rad) * cfg.rad_to_mm) @@ -372,6 +404,8 @@ def _judge_empty(result, trace: Trace, cfg, _motion) -> list[str]: def _judge_loaded(result, trace: Trace, cfg, motion) -> list[str]: """An object between the jaws: it should stall early and then hold.""" problems = [] + if result.no_bite: + return [_no_bite_problem(result, motion)] short = _stall_ramp_mm(result, trace, cfg, motion) if short is not None: # stalled could not have become True here, so saying it did not fire is diff --git a/src/litegrip/actions.py b/src/litegrip/actions.py index 84fa541..5c1ddc7 100644 --- a/src/litegrip/actions.py +++ b/src/litegrip/actions.py @@ -159,15 +159,29 @@ class MotionConfig: # 力,于是夹爪带着设定力在自由行程里一路加速(2026-10-29 实测:2 N 夹取 # 的接近段压 0.18 Nm 原地不动,接手的保力段压 0.20 Nm 推了 19 mm)。 # - # 所以带设定力的接近段里,位移小于这个值的停住**不算接触**:报「没能脱离 - # 静摩擦」(``MoveResult.no_bite``)、撤力、不交接。代价是「工件就贴在起点 - # 旁边、夹爪还没走开就顶上了」也按这个处理 —— 那一次夹取会报失败,而反过来 - # 放行会把一次误判变成一次带力冲程,两边代价不对称,所以选失败。 + # 所以带设定力的接近段里,位移小于这个值的停住,且预算压不出脱离力(见 + # ``breakaway_nm``)时**不算接触**:报「没能脱离静摩擦」(``MoveResult.no_bite``)、 + # 撤力、不交接。代价是「工件就贴在起点旁边、夹爪还没走开就顶上了」也按这个 + # 处理 —— 那一次夹取会报失败,而反过来放行会把一次误判变成一次带力冲程, + # 两边代价不对称,所以选失败。 # # 0.5 mm 是台架闭合侧粘滑死区(约 0.0103 rad ≈ 0.76 mm)的下面:比死区更短 # 的「顶住」读数本来就分辨不出来,真工件也做不到停在这么窄的缝里。 no_bite_mm: float = 0.5 + # ── 脱离力:判断「没动」是哪一种的力矩门槛 ──────────────────────── + # 上面那条判据只在**这一段的预算本来就压不出脱离力**时才成立。预算远高于 + # 脱离力时,夹爪没动几乎只可能是顶上了东西 —— 那正是「夹住了」,该交接, + # 保力段照着设定值压,不会跑。所以判据再收紧一层:只有预算 + # (``force_n × 0.1 × press_safety``) 低于这个值,零位移才判 ``no_bite``; + # 否则照旧 ``stalled``。少了这一层,20 N 夹一个已经贴在张开侧的工件、或者 + # ``release(abort=True)`` 之后原地重夹(那一次松开不发张开指令),都会被拒。 + # + # 0.2 Nm 就是这台机器上实测的脱离力(2026-10-29:接近段 0.18 Nm 原地不动, + # 保力段 0.20 Nm 推走 19 mm,落在 1.8–2.0 N 之间)。换成设定值:≤ 2.2 N 的 + # 夹取预算不到它,零位移判拒夹;≥ 2.5 N 的预算在它之上,零位移判顶住东西。 + breakaway_nm: float = 0.2 + # ── 行进段堵转保护(≈7 N) ──────────────────────────────────────── # 普通移动(``press=True`` 的 open/close)行进段的领先上限是 max_lead_mm, # 一旦夹爪在半路被硬挡,``kp × 领先上限`` ≈ 7.6 N·m 会一直压着;而位置 @@ -817,6 +831,14 @@ def grasp( 排程由摩擦和读数延迟定。两条都没成立时**如实报 ``ok=False``** 并撤力 返回(``reached`` / ``stalled`` 都是 ``False``,``cycles=0``),不放跑。 + 还有一条不算交接:**从起点就没动过**。带设定力的接近段如果停住时离它 + 自己的起点不到 :attr:`MotionConfig.no_bite_mm`(0.5 mm),而且这一段的 + 预算(``force_n × 0.1 × press_safety``)低于 + :attr:`MotionConfig.breakaway_nm`(0.2 Nm,实测脱离力),那它不是顶到 + 工件,而是压根没脱离静摩擦 —— 交过去的话保力段 1.0 倍的力矩正好越过 + 脱离力,夹爪会在自由行程里带着设定力一路走。这一条报 ``no_bite=True``、 + ``cycles=0``、撤力返回,一帧保力都不发(见 :attr:`MotionConfig.no_bite_mm`)。 + 保力段是纯力矩源,没有位置指令可夹,所以它有自己的一条护栏 (:attr:`MotionConfig.hold_runaway_rad`):越出闭合侧标定限位太多就撤力 返回,结果里 ``ok=False`` / ``travel_guard=True``。正常夹取不会触发。 @@ -831,8 +853,9 @@ def grasp( Returns: :class:`GraspResult`。夹住工件时 ``stalled=True`` 且 ``reached=False``(压不到空载目标位置是正常的)。闭合段结束时夹爪 - 既没到位也没堵转时 ``ok=False``、``cycles=0`` —— 见上。被松开结束 - 时 ``released=True``:正常松开 ``ok=True``,急停 ``ok=False``。 + 既没到位也没堵转时 ``ok=False``、``cycles=0`` —— 见上。接近段没能 + 脱离静摩擦时 ``ok=False``、``no_bite=True``、``cycles=0``。被松开 + 结束时 ``released=True``:正常松开 ``ok=True``,急停 ``ok=False``。 Raises: CommandError: ``force_n`` / ``hold_s`` 不是有限数。两条都不是 @@ -894,16 +917,7 @@ def grasp( "交过去会在自由行程里一路加速 —— 撤力返回,不下发保力。", force, force * UnitConversion.N_TO_NM * cfg.press_safety, move.state.position_mm) - return GraspResult( - ok=False, - reached=move.reached, - stalled=move.stalled, - state=move.state, - target_rad=move.target_rad, - force_n=force, - cycles=0, - no_bite=True, - ) + return self._refused_handover(move, force, no_bite=True) if not (move.reached or move.stalled): # 既没到位也没顶上东西 —— 夹爪还在自由行程里,而位置环是此刻 @@ -916,16 +930,7 @@ def grasp( "%+.5f rad,目标 %+.5f rad)—— 它还在自由行程里,而保" "力段没有位置项也没有速度项,交过去会一路加速。撤力返回。", move.state.position_rad, move.target_rad) - return GraspResult( - ok=False, - reached=move.reached, - stalled=move.stalled, - state=move.state, - target_rad=move.target_rad, - force_n=force, - cycles=0, - travel_guard=False, - ) + return self._refused_handover(move, force) # 交接点:带力接近段的终点(夹爪「声明」抓住的位置),不是随后被 # 20 N 压实后停下的位置。松开的基准取它,见 _do_release。 @@ -966,6 +971,31 @@ def grasp( self._grip_rad = None self._hold_done.set() + @staticmethod + def _refused_handover( + move: MoveResult, + force: float, + *, + no_bite: bool = False, + ) -> GraspResult: + """交接被拒时的 :class:`GraspResult`:一帧保力都没发。 + + 两条拒绝路径 —— 接近段从起点就没动过(``no_bite``),和闭合段结束时 + 既没到位也没堵转 —— 结果形状完全一样,只有 ``no_bite`` 不同。放在一处 + 构造,免得将来改了字段只改到一条(见 ``grasp()`` 里的两处调用)。 + """ + return GraspResult( + ok=False, + reached=move.reached, + stalled=move.stalled, + state=move.state, + target_rad=move.target_rad, + force_n=force, + cycles=0, + travel_guard=False, + no_bite=no_bite, + ) + def _grasp_ends_with_release( self, req: _ReleaseRequest, @@ -1580,6 +1610,11 @@ def _move_to_limit( # 整段斜坡都是「不动」。`home()` 紧跟在 `close()` 之后(两个都瞄着闭合 # 端,只剩一点点齿隙)就是这个形状。压紧路径不受影响:那里目标在限位 # 外侧,「到了还不动」正是要判的堵转。 + # 带设定力的接近段里,这一条还留了个小口子:起点落在 ``reach_tol`` + # (0.02 rad ≈ 0.9 mm)以内时不判堵转,于是 ``no_bite`` 也设不上 —— + # 但那时 ``reached`` 为真、夹爪已经在闭合端 0.9 mm 之内,保力段前面 + # 没有自由行程可跑,交过去最多把最后那点缝隙压到限位上。所以这条口子 + # 不补,README 说的是「从起点就没动过」不是「任何情况下都不交接」。 judge_stall = press or dist_mm > cfg.reach_tol * gcfg.rad_to_mm # 两档领先上限:行进段用 max_lead_mm 破静摩擦,贴近限位后收窄到 @@ -1708,8 +1743,12 @@ def _move_to_limit( # 「没动」不等于「顶到工件」:带设定力时,从起点起就没挪过位的 # 停住也可能是这一段压不出脱离静摩擦的力矩。这一条不能当接触, # 否则交接给纯力矩的保力段就是一次带力冲程(见 - # ``MotionConfig.no_bite_mm``)。 + # ``MotionConfig.no_bite_mm``)。但只有预算本来就在脱离力之下时 + # 才这么判 —— 预算远高于脱离力还没动,那是真顶上了东西 + # (见 ``MotionConfig.breakaway_nm``)。 if (force_n is not None + and force_n * UnitConversion.N_TO_NM * cfg.press_safety + < cfg.breakaway_nm and abs(pos - before.position_rad) < cfg.no_bite_mm / gcfg.rad_to_mm): no_bite = True diff --git a/src/litegrip/gripper.py b/src/litegrip/gripper.py index 2c76caf..dd71325 100644 --- a/src/litegrip/gripper.py +++ b/src/litegrip/gripper.py @@ -1393,7 +1393,12 @@ def grasp( it closed onto an object, which is the expected outcome. ``released=True`` means the grasp ended because a release was requested: ``ok=True`` for a normal release, ``ok=False`` for - an abort. + an abort. ``no_bite=True`` (with ``ok=False``, ``cycles=0``) + means the approach never moved the jaws and its torque budget + was below the breakaway: the hold was **not** entered, because + handing a jaw with a free stroke ahead of it to the torque-only + hold would power it across the gap. See + ``MotionConfig.no_bite_mm`` and ``.breakaway_nm``. """ self._check_connected() return self._actions.grasp(force_n, hold_s, progress=progress) diff --git a/tests/test_actions.py b/tests/test_actions.py index d04bc53..e2028e3 100644 --- a/tests/test_actions.py +++ b/tests/test_actions.py @@ -1888,24 +1888,55 @@ class TestTheApproachRefusesToHandOverWithoutBite(unittest.TestCase): 夹取点报在工件前 19–39 mm。操作者看到的是「先不动,然后飞速闭合」。 判据见 ``MotionConfig.no_bite_mm``:带设定力的接近段里,位移小于这个值的停住 - 不算接触 —— 报 ``no_bite``、撤力,保力段一帧都不发。 + 不算接触 —— 报 ``no_bite``、撤力,保力段一帧都不发。而且只在预算低于 + ``MotionConfig.breakaway_nm`` 时才这么判:预算远高于脱离力还没动,那是真顶上了 + 东西(见 ``test_a_workpiece_at_the_start_is_contact_above_the_breakaway``)。 """ - #: 脱离力:2 N 的预算(0.18 Nm)压不动,3 N 的(0.27 Nm)压得动。 - BREAKAWAY_NM = 0.22 + #: 实测脱离力落在 1.8–2.0 N,取 0.19 Nm(1.9 N):2 N 夹取的预算 0.18 Nm 压不动, + #: 3 N 的 0.27 Nm 压得动。 + BREAKAWAY_NM = 0.19 + #: 纯力矩的保力帧在假台上要靠这个才动(默认模型里 kp=0 一点不动)。 + CREEP = 5.0 - def _grasp(self, force_n, hold_s=0.2, **kw): + def _grasp(self, force_n, hold_s=0.2, *, no_bite_mm=None, **kw): motion = MotionConfig(sleep_fn=lambda _: None, monotonic_fn=tick_clock(0.1)) + if no_bite_mm is not None: + motion.no_bite_mm = no_bite_mm g, fake = make_gripper(stops=True, motion=motion, - breakaway_tau_nm=self.BREAKAWAY_NM, **kw) + breakaway_tau_nm=self.BREAKAWAY_NM, + creep_rad_s_per_nm=self.CREEP, **kw) return g, fake, g.grasp(force_n=force_n, hold_s=hold_s) @staticmethod def _budget(force_n): return force_n * UnitConversion.N_TO_NM * MotionConfig().press_safety - def test_the_setpoint_below_the_breakaway_is_refused_not_held(self): + @staticmethod + def _travelled_mm(fake): + return abs(fake.motor.pos - POS_OPEN_RAD) * RAD_TO_MM + + def test_the_old_handover_walks_the_jaws_across_the_whole_free_stroke(self): + """修复前的行为,钉在这里当对照:保力段把夹爪一路推到底。 + + ``no_bite_mm=0`` 关掉拒夹,就是修复前的样子。零位移被判成接触之后,保力 + 段的 0.20 Nm 越过了 0.19 Nm 的脱离力,夹爪在整段自由行程里被推着走 —— + 这正是台架上「先不动,然后飞速闭合」的那一台。下一条用例要证明的,就是 + 拒夹把这段行程拦在了开头。两条用的是同一个假台,只差一个 ``no_bite_mm``。 + """ + _g, fake, res = self._grasp(2.0, no_bite_mm=0.0) + + self.assertTrue(res.stalled, res) # 修复前:零位移被当成接触 + self.assertFalse(res.no_bite, res) + self.assertGreater(res.cycles, 0, res) + # 0.2 s 的保力就推了十几毫米(台架那次是 19 mm,夹取点报在工件前), + # 而修复后同一个动作一毫米都不走 —— 对比就在下一条用例。 + self.assertGreater(self._travelled_mm(fake), 10.0, + f"保力段应该把夹爪推着走,实际只走了 " + f"{self._travelled_mm(fake):.1f} mm") + + def test_the_refusal_stops_that_stroke_before_it_starts(self): _g, fake, res = self._grasp(2.0) self.assertLess(self._budget(2.0), self.BREAKAWAY_NM) # 前提 @@ -1914,6 +1945,8 @@ def test_the_setpoint_below_the_breakaway_is_refused_not_held(self): self.assertFalse(res.stalled, res) # 停住 ≠ 顶到工件 self.assertFalse(res.reached, res) self.assertEqual(res.cycles, 0) + self.assertLess(self._travelled_mm(fake), MotionConfig().no_bite_mm, + "拒夹之后夹爪不许离开起点") self.assertEqual([f for f in fake.frames if f.tau_ff != 0.0], [], "保力段一帧都不许发:它是纯力矩源,没有位置也没有限速") self.assertEqual(fake.frames[-1].kp, 0.0, "结尾要撤力") @@ -1927,20 +1960,40 @@ def test_the_setpoint_above_the_breakaway_still_grasps(self): self.assertTrue(res.ok, res) self.assertGreater(res.cycles, 0) - def test_a_workpiece_at_the_start_is_refused_as_well(self): - """代价写在测试里:工件就贴在起点时,这一次夹取也会被拒。 + def test_a_workpiece_at_the_start_is_contact_above_the_breakaway(self): + """预算远高于脱离力时,零位移是**顶上东西**,不是没能脱离。 - 电机分不出「顶住工件」和「压不出脱离力」—— 两者都是零位移。拒夹的代价 - 是操作者看到一次失败、重摆一次;放行的代价是一次带力冲程(上面那个 19 mm - 的推程)。两边不对称,所以选拒夹。 + 电机分不出这两种零位移,判据改用预算来分:0.20 Nm 以上还不动,只可能是 + 有东西挡着。这时该交接 —— 保力段照着设定值压,压的就是用户要的力。少了 + 这一条,一个已经贴在张开侧的工件、以及 ``release(abort=True)`` 之后原地 + 重夹(那一次松开不发张开指令),都会变成一次失败的夹取。 """ start = POS_OPEN_RAD - block = start + 0.001 * (POS_CLOSED_RAD - POS_OPEN_RAD) # ≈ 0.09 mm + block = start + 0.001 * (POS_CLOSED_RAD - POS_OPEN_RAD) # ≈ 0.08 mm motion = MotionConfig(sleep_fn=lambda _: None, monotonic_fn=tick_clock(0.1)) - g, fake = make_gripper(stops=True, motion=motion, block_rad=block) + g, _fake = make_gripper(stops=True, motion=motion, block_rad=block) res = g.grasp(force_n=20.0, hold_s=0.2) + self.assertFalse(res.no_bite, res) + self.assertTrue(res.stalled, res) + self.assertTrue(res.ok, res) + self.assertGreater(res.cycles, 0) + + def test_a_workpiece_at_the_start_is_refused_below_the_breakaway(self): + """代价写在测试里:预算不到脱离力时,贴在起点上的工件也会被拒。 + + 和上一条同一块挡块、同一个起点,只把设定值降到 2 N。拒夹的代价是操作者 + 看到一次失败、重摆一次;放行的代价是一次带力冲程(上一条用例里的整段)。 + 两边不对称,所以选拒夹。 + """ + start = POS_OPEN_RAD + block = start + 0.001 * (POS_CLOSED_RAD - POS_OPEN_RAD) + motion = MotionConfig(sleep_fn=lambda _: None, + monotonic_fn=tick_clock(0.1)) + g, fake = make_gripper(stops=True, motion=motion, block_rad=block) + res = g.grasp(force_n=2.0, hold_s=0.2) + self.assertTrue(res.no_bite, res) self.assertFalse(res.ok, res) self.assertEqual([f for f in fake.frames if f.tau_ff != 0.0], [])