diff --git a/README.md b/README.md index a32ff11..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). `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. | @@ -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,25 @@ 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 *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 (~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` and `stalled` still describe the closing leg; `state` is the pose after the release. @@ -447,6 +466,8 @@ 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) | +| `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 aae8a6e..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`(空爪闭到底)。`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` 的默认值更长。 | @@ -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,22 @@ 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),**而且**这一段的预算 + (`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`。 @@ -386,6 +402,8 @@ 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()`) | +| `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 d62b007..5c1ddc7 100644 --- a/src/litegrip/actions.py +++ b/src/litegrip/actions.py @@ -151,6 +151,37 @@ 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)。 + # + # 所以带设定力的接近段里,位移小于这个值的停住,且预算压不出脱离力(见 + # ``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 会一直压着;而位置 @@ -276,6 +307,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 +338,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 @@ -790,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``。正常夹取不会触发。 @@ -804,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`` 不是有限数。两条都不是 @@ -854,6 +904,21 @@ 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 self._refused_handover(move, force, no_bite=True) + if not (move.reached or move.stalled): # 既没到位也没顶上东西 —— 夹爪还在自由行程里,而位置环是此刻 # 唯一拦着它的东西(kp × 领先 + kd × 速度误差)。保力段的帧里 @@ -865,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。 @@ -915,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, @@ -1529,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 破静摩擦,贴近限位后收窄到 @@ -1558,6 +1644,7 @@ def _move_to_limit( hist = [before.position_rad] stalled = False + no_bite = False # 带力接近段从起点就没动过(见上) protected = False release_requested = False # 松开请求叫停了带力接近段 over_torque = 0 # 连续过阈的采样点数 @@ -1653,10 +1740,31 @@ 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``)。但只有预算本来就在脱离力之下时 + # 才这么判 —— 预算远高于脱离力还没动,那是真顶上了东西 + # (见 ``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 + 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 +1811,7 @@ def _move_to_limit( steps=last_i, protected=protected, release_requested=release_requested, + no_bite=no_bite, ) def _hold_force( 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/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..e2028e3 100644 --- a/tests/test_actions.py +++ b/tests/test_actions.py @@ -1879,6 +1879,126 @@ 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``、撤力,保力段一帧都不发。而且只在预算低于 + ``MotionConfig.breakaway_nm`` 时才这么判:预算远高于脱离力还没动,那是真顶上了 + 东西(见 ``test_a_workpiece_at_the_start_is_contact_above_the_breakaway``)。 + """ + + #: 实测脱离力落在 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, *, 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, + 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 + + @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) # 前提 + 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.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, "结尾要撤力") + + 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_contact_above_the_breakaway(self): + """预算远高于脱离力时,零位移是**顶上东西**,不是没能脱离。 + + 电机分不出这两种零位移,判据改用预算来分:0.20 Nm 以上还不动,只可能是 + 有东西挡着。这时该交接 —— 保力段照着设定值压,压的就是用户要的力。少了 + 这一条,一个已经贴在张开侧的工件、以及 ``release(abort=True)`` 之后原地 + 重夹(那一次松开不发张开指令),都会变成一次失败的夹取。 + """ + start = POS_OPEN_RAD + 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) + 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], []) + + class TestNonFiniteInputIsRefused(unittest.TestCase): """非有限值不许变成运动:拒绝带原因,而且**一帧都不发**。