Skip to content

fix: bound the calibration probe and surface its errors #14

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
yd-sl merged 1 commit into from
Sep 29, 2026
Merged

fix: bound the calibration probe and surface its errors #14

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 14 additions & 5 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -232,9 +232,10 @@ with LiteGrip("can0") as gripper:
| `hold_interval` | `0.2` | force-hold slice length, seconds |
| `hold_kp` / `hold_kd` | `150.0` / `2.0` | gains used while holding force |
| `enable_retries` / `enable_retry_interval` | `3` / `0.2` | enable retry count and gap |
| `calib_kp` / `calib_kd` | `60.0` / `2.0` | probe stiffness used by `zero()` |
| `calib_step_rad` | `0.1` | probe increment used by `zero()` |
| `calib_stall_delta` / `calib_stall_cycles` / `calib_max_iter` | `0.0015` / `5` / `80` | probe stall criteria |
| `calib_kp` / `calib_kd` | `20.0` / `2.0` | probe stiffness used by `zero()` |
| `calib_step_rad` | `0.05` | probe increment used by `zero()`, and the cap on how far the command may lead the measured position |
| `calib_tau_limit` | `2.0` | probe torque ceiling, Nm — the probe freezes as soon as `\|tau\|` reaches it |
| `calib_stall_delta` / `calib_stall_cycles` / `calib_max_iter` | `0.0015` / `5` / `200` | probe stall criteria |
| `sleep_fn` / `monotonic_fn` | `time.sleep` / `time.monotonic` | seams for tests and simulation |

`sleep_fn` and `monotonic_fn` are the supported way to simulate the gripper: the engine calls
Expand Down Expand Up @@ -361,8 +362,16 @@ These behaviour changes matter beyond the signatures:
- `enable()` now reports failure honestly. It used to return `True` whenever the status frame
held `0` or `1`, so a motor that never energised looked enabled. Code that ignored the return
value and carried on will now see a `HardwareError` at startup instead.
- `zero()` is not `calibrate()`. It probes with the `MotionConfig.calib_*` values and saves the
result, whereas `calibrate()` keeps its own older defaults and does not save.
- `zero()` is not `calibrate()`. Both now probe with the same defaults and both bound the command
lead to one step and stop at `tau_limit`; the difference is that `zero()` probes with the
`MotionConfig.calib_*` values and **saves** the result, while `calibrate()` takes its arguments
directly and does not save.
- The probe is safe at a hard stop. It used to advance its target unconditionally, so once the
jaws reached a stop the command kept leading further every cycle and `kp × error` kept growing
until the structure gave way. The command is now re-derived from the measured position each
cycle (lead ≤ `calib_step_rad`), and the probe aborts the instant `|tau|` reaches
`calib_tau_limit` — a guard that does not depend on the position-based stall test, which cannot
fire while the structure is still yielding.
- `MoveResult.__bool__` used to be `reached and not stalled`; it is now `ok`. For `grasp`'s
closing phase the two agree. For `open` and `close` they are opposite: a successful press onto
the stop is `stalled=True, reached=False`, so `if gripper.close():` means something different
Expand Down
16 changes: 11 additions & 5 deletions readme_zn.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -215,9 +215,10 @@ with LiteGrip("can0") as gripper:
| `hold_interval` | `0.2` | 保力的分片时长 s |
| `hold_kp` / `hold_kd` | `150.0` / `2.0` | 保力时用的刚度 / 阻尼 |
| `enable_retries` / `enable_retry_interval` | `3` / `0.2` | 使能重试次数与间隔 |
| `calib_kp` / `calib_kd` | `60.0` / `2.0` | `zero()` 探测用的刚度 / 阻尼 |
| `calib_step_rad` | `0.1` | `zero()` 的探测步长 |
| `calib_stall_delta` / `calib_stall_cycles` / `calib_max_iter` | `0.0015` / `5` / `80` | 探测的堵转判据 |
| `calib_kp` / `calib_kd` | `20.0` / `2.0` | `zero()` 探测用的刚度 / 阻尼 |
| `calib_step_rad` | `0.05` | `zero()` 的探测步长，同时也是指令领先实测位置的上限 |
| `calib_tau_limit` | `2.0` | 探测的力矩上限 Nm —— `\|tau\|` 一到就停 |
| `calib_stall_delta` / `calib_stall_cycles` / `calib_max_iter` | `0.0015` / `5` / `200` | 探测的堵转判据 |
| `sleep_fn` / `monotonic_fn` | `time.sleep` / `time.monotonic` | 给测试和仿真留的缝 |

`sleep_fn` 与 `monotonic_fn` 是官方推荐的仿真入口：引擎内部只调这两个，所以传
Expand Down Expand Up @@ -327,8 +328,13 @@ with LiteGrip("can0") as gripper:
- `enable()` 现在会如实报失败。以前只要状态帧是 `0` 或 `1` 它就返回 `True`，于是「根本没
使能」的电机看起来是使能的。那些忽略返回值继续往下跑的代码，现在会在启动时看到
`HardwareError`。
- `zero()` 不等于 `calibrate()`。它按 `MotionConfig.calib_*` 的参数探测并保存结果，而
`calibrate()` 保留着自己那套更旧的默认值，而且不存盘。
- `zero()` 不等于 `calibrate()`。两者现在用同一套默认值、都把指令领先量限成一步、
都在 `tau_limit` 到顶时停下；区别只在于 `zero()` 取 `MotionConfig.calib_*` 并**存盘**，
而 `calibrate()` 直接收参数、不存盘。
- 探测在硬限位处是安全的。旧实现无条件外推目标，顶住之后指令每拍继续领先，`kp × 误差`
随之上涨直到结构崩掉。现在每个循环都从实测位置重新算目标（领先量 ≤ `calib_step_rad`），
且 `|tau|` 一到 `calib_tau_limit` 立刻中止 —— 这道护栏不依赖位置式堵转判据，后者在结构
还在让位时永远不会成立。
- `MoveResult.__bool__` 以前是 `reached and not stalled`，现在是 `ok`。对 `grasp` 闭合段
两者一致；对 `open` 和 `close` 恰好相反 —— 成功顶到限位是 `stalled=True,
reached=False`，所以 `if gripper.close():` 的含义变了，尽管类型没变。
Expand Down
12 changes: 7 additions & 5 deletions src/litegrip/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,6 @@
Use directly if you need fine-grained control or multi-motor setups.
"""

import logging

# Version of a package that is on disk but not installed as a distribution — a
# source checkout, a vendored copy, or the files dropped in by a system package.
# The ``+`` local segment keeps it valid PEP 440 and makes it obvious in a bug
Expand Down Expand Up @@ -55,9 +53,13 @@ def _detect_version(dist_name: str = "litegrip") -> str:

__version__ = _detect_version()

# Ensure log messages are visible even if the user hasn't configured logging.
_log = logging.getLogger("litegrip")
_log.addHandler(logging.NullHandler())
# This package deliberately installs no handler and does not call
# ``logging.disable()``: a WARNING such as "this calibration file belongs to
# another channel" is a safety signal, and must reach the user even when the
# embedding program never configured logging. With no handler, Python's
# ``logging.lastResort`` prints WARNING and above to stderr. Applications that
# want to capture the records can still attach their own handler to the
# ``litegrip`` logger.

# ── High-level API ──────────────────────────────────────────────────────
from .gripper import (LiteGrip, DEFAULT_CALIB, CALIB_TEMPLATES,
Expand Down
15 changes: 12 additions & 3 deletions src/litegrip/actions.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,12 +104,17 @@ class MotionConfig:
enable_retry_interval: float = 0.2 # 使能重试间隔 s

# ── zero() 标定探测 ────────────────────────────────────────────────
calib_kp: float = 60.0 # 低刚度更温和
# 探测顶在限位上的力矩就是 calib_kp × 指令领先量，而领先量本身被
# _find_limit 限成 calib_step_rad（目标只比实测位置多一步），所以这两个
# 值同时决定「走多快」和「顶多重」：20 × 0.05 ⇒ 空载推进约 1 Nm，
# 远低于 DM4310 的峰值。calib_tau_limit 是独立于堵转判据的硬上限。
calib_kp: float = 20.0 # 低刚度更温和（力矩 = kp × 领先量）
calib_kd: float = 2.0
calib_step_rad: float = 0.1 # 每步步进 rad
calib_step_rad: float = 0.05 # 每步步进 rad（= 指令领先上限）
calib_tau_limit: float = 2.0 # 力矩上限 Nm，超过即停
calib_stall_delta: float = 0.0015 # 标定堵转判据 rad
calib_stall_cycles: int = 5 # 标定连续堵转次数
calib_max_iter: int = 80 # 单向步数上限
calib_max_iter: int = 200 # 单向步数上限

# ── 测试/仿真缝 ────────────────────────────────────────────────────
sleep_fn: Callable[[float], None] = field(
Expand Down Expand Up @@ -382,6 +387,9 @@ def zero(self) -> CalibrationData:
"""完整标定：探闭合 + 张开两个限位，算出行程与换算系数，并存盘。

过程中夹爪会主动顶住两端机械限位（低刚度探测）。确保行程内无物。
探测自带两道护栏：指令领先量不超过 ``calib_step_rad``，且 ``|tau|`` 一到
``calib_tau_limit`` 立即停止推进 —— 顶住限位时结构让位（背隙/弹性变形）
会让位置读数一直在动，只靠「位置不再变化」是停不下来的。

Returns:
:class:`CalibrationData`。
Expand All @@ -394,6 +402,7 @@ def zero(self) -> CalibrationData:
stall_delta=cfg.calib_stall_delta,
stall_cycles=cfg.calib_stall_cycles,
max_iter=cfg.calib_max_iter,
tau_limit=cfg.calib_tau_limit,
)
self._g.save_calibration()
return data
Expand Down
113 changes: 91 additions & 22 deletions src/litegrip/gripper.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -721,12 +721,13 @@ def _label(rad: float) -> str:

def calibrate_guided(
self,
kp: float = 60.0,
kp: float = 20.0,
kd: float = 2.0,
step_rad: float = 0.08,
stall_delta: float = 0.0004,
stall_cycles: int = 6,
max_iter: int = 40,
step_rad: float = 0.05,
stall_delta: float = 0.0015,
stall_cycles: int = 5,
max_iter: int = 200,
tau_limit: Optional[float] = 2.0,
) -> CalibrationData:
"""Guided two-step calibration with user confirmation at each limit.

Expand All @@ -748,6 +749,10 @@ def calibrate_guided(
stall_delta: Position delta for auto-stall detection (rad).
stall_cycles: Consecutive stalls to auto-confirm limit.
max_iter: Max steps per direction.
tau_limit: Torque ceiling in Nm; the probe stops when ``|tau|``
reaches it (``None`` disables the ceiling). The command lead is
already bounded to ``step_rad`` here, so this is the second,
independent guard.

Returns:
CalibrationData; :attr:`config` is updated in-place.
Expand Down Expand Up @@ -785,6 +790,7 @@ def _step_to_limit(direction: str, sign: float, label: str) -> float:

new_pos = self._can.get_position()
delta = abs(new_pos - current)
tau = self._can.get_torque()

# Non-blocking keyboard check
hit_enter = False
Expand All @@ -795,11 +801,16 @@ def _step_to_limit(direction: str, sign: float, label: str) -> float:
hit_enter = True

print(f" [{i}] pos={new_pos:.4f} rad d={delta:.5f} "
f"stall={stall}", end="")
f"tau={tau:+.3f} stall={stall}", end="")
if hit_enter:
print(" ← 用户确认")
return new_pos

if tau_limit is not None and abs(tau) >= tau_limit:
print(f" ← 力矩达到上限 {tau_limit:.2f} Nm（{tau:+.3f}）")
print(f" → 停止推进并保持: {new_pos:.6f} rad")
return new_pos

if delta < stall_delta:
stall += 1
print(f" (堵转检测中)")
Expand Down Expand Up @@ -897,7 +908,11 @@ def save_calibration(self, path: Optional[str] = None) -> str:
data = {
"channel": self._channel,
"can_id": self._can_id,
"mst_id": self._mst_id or 0,
# Only stamp mst_id when it is actually known. Writing a falsy 0
# would pin auto-detection: load_calibration would then install a
# RX filter of 0x000, every reply from the motor would be dropped,
# and enable() would fail after a long retry loop. Omitting it
# keeps the value "unknown", which connect() reads as auto-detect.
"canfd_mode": self._canfd_mode or False,
"calibrated": True,
# Which of zero/max is numerically larger is what carries the
Expand All @@ -911,6 +926,8 @@ def save_calibration(self, path: Optional[str] = None) -> str:
"kd": self._config.kd,
"grasp_torque_threshold": self._config.grasp_torque_threshold,
}
if self._mst_id:
data["mst_id"] = self._mst_id
parent = _os.path.dirname(path)
if parent:
_os.makedirs(parent, exist_ok=True)
Expand Down Expand Up @@ -1039,10 +1056,13 @@ def load_calibration(self, path: Optional[str] = None,
"同一台电脑上多台夹爪共用 CAN ID 时，通道是唯一身份键，"
"确认没有指错文件。", file_channel, self._channel)

# Also update instance-level IDs if present
if "can_id" in data:
# Also update instance-level IDs if present. A falsy value counts as
# "unknown", not as ID 0: files written before this SDK started omitting
# an unset mst_id (and hand-edited ones) may carry ``"mst_id": 0``, which
# would otherwise pin the CAN RX filter to 0x000 and drop every reply.
if data.get("can_id"):
self._can_id = int(data["can_id"])
if "mst_id" in data:
if data.get("mst_id"):
self._mst_id = int(data["mst_id"])

s = self._config.close_sign
Expand Down Expand Up @@ -1410,12 +1430,13 @@ def _move_at_speed_rad(

def calibrate(
self,
kp: float = 60.0,
kp: float = 20.0,
kd: float = 2.0,
step_rad: float = 0.1,
stall_delta: float = 0.0003,
stall_cycles: int = 8,
max_iter: int = 30,
step_rad: float = 0.05,
stall_delta: float = 0.0015,
stall_cycles: int = 5,
max_iter: int = 200,
tau_limit: Optional[float] = 2.0,
) -> CalibrationData:
"""Calibrate the gripper: find close and open mechanical limits.

Expand All @@ -1431,13 +1452,24 @@ def calibrate(
comes from :attr:`GripperConfig.close_sign` (load a calibration
template first for a reverse mount), and this routine preserves it.

Safety: at a hard stop the encoder keeps creeping (backlash, elastic
deformation, micro-slip), so the position-based stall test alone can
never fire — the command would keep advancing and ``kp × error`` would
keep growing until the structure breaks. Two independent guards are
therefore in place: the command lead is re-derived from the *measured*
position every cycle (bounded to ``step_rad``, so pressing torque is at
most ``kp × step_rad``), and the probe aborts the moment ``|tau|``
reaches ``tau_limit``.

Args:
kp: Probing stiffness (low = gentle).
kd: Probing damping.
step_rad: Step size per iteration (rad).
step_rad: Step size per iteration (rad); also the command lead cap.
stall_delta: Position change threshold for stall detection (rad).
stall_cycles: Consecutive stalls to confirm limit.
max_iter: Max steps per direction (safety cap).
tau_limit: Torque ceiling in Nm. The probe stops as soon as
``|tau|`` reaches this; ``None`` disables the ceiling.

Returns:
CalibrationData with zero/max position, travel range, and
Expand All @@ -1462,11 +1494,15 @@ def _find_limit(direction: str) -> float:

self._can.update_state(timeout_s=0.05)
current = self._can.get_position()
target = current
stall = 0

for i in range(max_iter):
target += sign * step_rad
# Re-derive the command from the *measured* position each cycle.
# Accumulating a running target (``target += sign * step_rad``)
# lets the command lead grow without bound once the stop is
# reached, and the pressing torque (kp × lead) grows with it.
# Bounding the lead to one step caps it at kp × step_rad.
target = current + sign * step_rad
self._can.control_mit_stream(target, kp, kd, duration_s=0.3, interval_s=0.005)
self._can.update_state(timeout_s=0.1)

Expand All @@ -1477,6 +1513,11 @@ def _find_limit(direction: str) -> float:
print(f" [{i}] tgt={target:+.3f} pos={new_pos:.4f} "
f"d={delta:.5f} tau={tau:+.3f} st={stall}")

if tau_limit is not None and abs(tau) >= tau_limit:
print(f" → 力矩达到上限 {tau_limit:.2f} Nm（{tau:+.3f}），"
f"停止推进并保持: {new_pos:.6f} rad")
return new_pos

if delta < stall_delta:
stall += 1
if stall >= stall_cycles:
Expand All @@ -1489,19 +1530,47 @@ def _find_limit(direction: str) -> float:
print(f" → 安全停止（达到最大步数 {max_iter}）: {current:.4f} rad")
return current

def _bounded_move(target: float, label: str) -> None:
"""Move toward *target*, guarded the same way as the probe.

The plain ``goto_rad`` streams one constant command for half a
second with no ceiling. If the jaws already sit at the stop that
command is heading for, the pressing torque is ``kp × 0.2`` (16 Nm at
``kp=80``) held for the whole duration — the same kind of unguarded
push that broke a jaw. Leading by one step at a time and watching
``|tau|`` bounds it to the probe's ceiling instead.
"""
current = self._can.get_position()
sign = 1.0 if target >= current else -1.0
for _ in range(max_iter): # same cap as the probe
if abs(target - current) <= step_rad:
return
self._can.control_mit_stream(
current + sign * step_rad, kp, kd,
duration_s=0.1, interval_s=0.005)
self._can.update_state(timeout_s=0.1)
new_pos = self._can.get_position()
tau = self._can.get_torque()
if tau_limit is not None and abs(tau) >= tau_limit:
print(f" → {label}：力矩达到上限 {tau_limit:.2f} Nm"
f"（{tau:+.3f}），停止")
return
if abs(new_pos - current) < stall_delta:
print(f" → {label}：位置不再变化，停止")
return
current = new_pos

# 1. Safe back-off (a nudge toward the close side, as before, flipped
# with the mount)
print(" 安全回退...")
self.goto_rad(init_pos + s * 0.2, kp=80, kd=kd, duration=0.5)
self._can.update_state(timeout_s=0.1)
_bounded_move(init_pos + s * 0.2, "安全回退")

# 2. Find zero (close direction)
zero_pos = _find_limit("close")

# 3. Back off, away from the close stop toward the open side
print(" 回退...")
self.goto_rad(zero_pos - s * 0.3, kp=80, kd=kd, duration=0.5)
self._can.update_state(timeout_s=0.1)
_bounded_move(zero_pos - s * 0.3, "回退")

# 4. Find max (open direction)
max_pos = _find_limit("open")
Expand Down
Loading
Loading