diff --git a/README.md b/README.md index a9590bf..7f74371 100644 --- a/README.md +++ b/README.md @@ -174,7 +174,7 @@ young. Ask both when the difference matters. | `open(speed_mm_s=None, *, progress=None)` → `MoveResult` | Ramps toward the calibrated open-side stop. With `GripperConfig.work_stroke_mm` set it stops at that opening instead — the shipped calibration sets `80.0` against the unit's 85 mm of travel, so a plain `open()` ends 5 mm short of the open end. | | `close(speed_mm_s=None, *, progress=None)` → `MoveResult` | The same, toward the closed side. `work_stroke_mm` is an open-side number and does not change where a close ends. | | `home()` → `MoveResult` | Ramps to the calibrated closed position. | -| `grasp(force_n=None, hold_s=0.0, *, progress=None)` → `GraspResult` | Closes onto the object — or onto the closed-side calibrated stop if there is nothing there — then ramps to and holds `force_n` N. The closing leg carries `force_n` as its torque budget, so meeting the object presses no harder than the setpoint. The hold takes over only once the jaw is stopped: it is `stalled=True` (something is between the jaws) or `reached=True` (the jaws closed empty). A stop that came **without the jaw ever moving from where the approach began**, with a budget below `MotionConfig.breakaway_nm`, is not treated as contact — the budget simply cannot break static friction — so the call releases in place and returns `no_bite=True` instead of handing over the hold. `hold_s=0` holds until a fault, the runaway watch, Ctrl+C, or a `release()` / `request_release()` from another thread — which ends the grasp with `released=True`. `force_n=None` uses `MotionConfig.force_n` (20 N). | +| `grasp(force_n=None, hold_s=0.0, *, progress=None)` → `GraspResult` | Closes onto the object — or onto the closed-side calibrated stop if there is nothing there — then ramps to and holds `force_n` N. The closing leg carries `force_n` as its torque budget, so meeting the object presses no harder than the setpoint. The hold takes over only once the jaw is stopped: it is `stalled=True` (something is between the jaws) or `reached=True` (the jaws closed empty). In the force approach `stalled=True` also requires the window's average speed to be under `MotionConfig.stall_speed_mm_s` (2 mm/s), so a jaw that is still creeping is not mistaken for contact and handed to the torque-only hold. A stop that came **without the jaw ever moving from where the approach began**, with a budget below `MotionConfig.breakaway_nm`, is not treated as contact — the budget simply cannot break static friction — so the call releases in place and returns `no_bite=True` instead of handing over the hold. `hold_s=0` holds until a fault, the runaway watch, Ctrl+C, or a `release()` / `request_release()` from another thread — which ends the grasp with `released=True`. `force_n=None` uses `MotionConfig.force_n` (20 N). | | `release(clearance_mm=10.0, *, abort=False, timeout_s=None)` → `ReleaseResult` | Ends an in-progress `grasp()` and opens the jaws from the pose where the grip was declared, by `clearance_mm` more, clamped to the calibrated open stop. Safe to call from any thread while `grasp(hold_s=0)` is blocked in another: it queues the request, the grasp thread performs the opening, and the call returns that outcome. With no grasp running it opens from the current position in the calling thread. `abort=True` is the E-stop form: torque off, no opening move, `ok=False`. A second call during the same grasp joins the first. | | `request_release(clearance_mm=10.0, *, abort=False)` → `bool` | Non-blocking form of `release()`: records the request if a grasp is running and returns `True`, else `False`. Never sends a frame, so it is safe for a UI thread, a signal handler or an E-stop callback. The grasp thread acts on it before its next frame (within 5 ms). | | `goto(position_mm, kp=None, kd=None, duration=0.5)` → `MoveResult` | Ramp to a position in millimetres. | @@ -387,7 +387,10 @@ What it *means*, and what to check, is per action: A fourth combination is a failure, and new in this version: - **`grasp()` with `ok=False, reached=False, stalled=False`** means the closing leg ran out of - frames without reaching the stop and without stalling — the jaw is still in free travel. The + frames without reaching the stop and without stalling — the jaw is still in free travel. A + slow creep lands here too: the window's net displacement is small, but its average speed is + still above `MotionConfig.stall_speed_mm_s` (2 mm/s), so it never counts as a stop and the leg + runs to its frame ceiling. The hold is *not* entered: its frames carry no position and no velocity term, so handing a moving jaw over to them leaves the feed-forward torque as the only thing acting on it, and the jaw accelerates until the runaway watch releases it. The call releases in place instead and reports @@ -709,6 +712,7 @@ Every script below is runnable from the repository root and prints what it measu | `examples/stroke_test.py` | Does this unit work? Both ends of the travel, exit 0 on success. | | `examples/grasp_test.py` | Does `grasp()` still do what it says — on clear jaws and on an object? | | `examples/hold_force_test.py` | Does the held grip hold its force, or drift to a smaller one? | +| `examples/api_test.py` | Interactive smoke test of the common high-level calls: read, enable, open, close, goto, grasp, release, stop, disable. | | `examples/zero_closed.py` | Low-level CAN bench tool, one subcommand per operation: `status`, `release`, `goto`, `zero`, `open`, `close`, `enable`, `disable`. | | `examples/teleop.py` | One end of a leader/follower link. | | `examples/trajectory.py` | Hand-teach a motion, save it, play it back. | diff --git a/README.zh-CN.md b/README.zh-CN.md index 58e8ad7..16949d0 100644 --- a/README.zh-CN.md +++ b/README.zh-CN.md @@ -163,7 +163,7 @@ print(state.position_mm, state.force_n, state.timestamp) | `open(speed_mm_s=None, *, progress=None)` → `MoveResult` | 爬坡走向标定的张开侧限位。若 `GripperConfig.work_stroke_mm` 有值,则停在那段行程处 —— 出厂标定设的是 `80.0`,对着这台机器 85 mm 的行程,所以裸调 `open()` 会在距张开端 5 mm 处停下。 | | `close(speed_mm_s=None, *, progress=None)` → `MoveResult` | 一样,方向朝闭合侧。`work_stroke_mm` 是张开侧的数字,不改变 `close()` 的落点。 | | `home()` → `MoveResult` | 爬坡回到标定的闭合位置。 | -| `grasp(force_n=None, hold_s=0.0, *, progress=None)` → `GraspResult` | 闭到工件上(中间没东西就闭到闭合侧标定限位),再爬到 `force_n` N 并保持。闭合段就把 `force_n` 当力矩预算,所以顶到工件时压得不会比设定值更狠。保力只在爪子**停下来**之后接手:`stalled=True`(爪子间夹着东西)或 `reached=True`(空爪闭到底)。但**从起点就没动过**就停下、且预算低于 `MotionConfig.breakaway_nm` 时不算接触 —— 那是预算压不出静摩擦 —— 这种情形调用原地撤力、返回 `no_bite=True`,不进保力段。`hold_s=0` 表示保持到故障、失控监视触发或 Ctrl+C。`force_n=None` 用 `MotionConfig.force_n`(20 N)。 | +| `grasp(force_n=None, hold_s=0.0, *, progress=None)` → `GraspResult` | 闭到工件上(中间没东西就闭到闭合侧标定限位),再爬到 `force_n` N 并保持。闭合段就把 `force_n` 当力矩预算,所以顶到工件时压得不会比设定值更狠。保力只在爪子**停下来**之后接手:`stalled=True`(爪子间夹着东西)或 `reached=True`(空爪闭到底)。带力接近段里 `stalled=True` 还要求窗口平均速度低于 `MotionConfig.stall_speed_mm_s`(2 mm/s)—— 还在慢速蠕动的夹爪不算接触,不交给纯力矩的保力段。但**从起点就没动过**就停下、且预算低于 `MotionConfig.breakaway_nm` 时不算接触 —— 那是预算压不出静摩擦 —— 这种情形调用原地撤力、返回 `no_bite=True`,不进保力段。`hold_s=0` 表示保持到故障、失控监视触发或 Ctrl+C。`force_n=None` 用 `MotionConfig.force_n`(20 N)。 | | `goto(position_mm, kp=None, kd=None, duration=0.5)` → `MoveResult` | 爬坡走到毫米目标。 | | `goto_rad(position_rad, kp=None, kd=None, dq_target=0.0, tau_feedforward=0.0, duration=0.5)` → `MoveResult` | 一样的,单位 rad。`dq_target` 与 `tau_feedforward` 必须是 0:速度前馈由斜坡按自己的排程给出,所以这两个值一律拒绝并抛 `CommandError`,不是静默忽略。 | | `move_to(target_rad, kp=None, kd=None, tau_feedforward=0.0, duration=1.0)` → `MoveResult` | 和 `goto_rad` 一样,只是 `duration` 的默认值更长。 | @@ -333,7 +333,9 @@ print(abs(cfg.pos_open_rad - cfg.pos_closed_rad) * cfg.rad_to_mm) # 行程,m 第四种组合是失败,也是这一版新增的: - **`grasp()` 返回 `ok=False, reached=False, stalled=False`**,意思是闭合段帧数用完了,既没 - 走到限位也没堵转 —— 爪子还在自由行程里。这一路**不会**进保力段:保力帧既没有位置项也没有 + 走到限位也没堵转 —— 爪子还在自由行程里。慢速蠕动也归这一路:窗口净位移虽小,平均速度仍 + 高于 `MotionConfig.stall_speed_mm_s`(2 mm/s),不算真停,闭合段一直走到帧数上界、按失败 + 收尾。这一路**不会**进保力段:保力帧既没有位置项也没有 速度项,把一个还在走的爪子交给它们,压在它身上的就只剩前馈力矩,它会一路加速直到失控监视 撤力。此时调用改为原地撤力,报 `cycles=0`。什么也没夹住。见到它就说明闭合段被饿着了 —— 读数过期,或者机构自身预算吃不掉的摩擦。 @@ -621,6 +623,7 @@ logger 挂个 handler 就能看到,注意即使不挂 handler,告警也会 | `examples/stroke_test.py` | 这台机器能不能用?走完行程两端,成功时退出码 0。 | | `examples/grasp_test.py` | `grasp()` 说的和做的还一致吗 —— 空爪和夹着东西各一遍? | | `examples/hold_force_test.py` | 保住的力是稳住了,还是漂到了更小的值? | +| `examples/api_test.py` | 常用高层 API 的交互冒烟测试:读取状态,以及 `enable()`、`open()`、`close()`、`goto()`、`grasp()`、`release()`、`stop()`、`disable()`。 | | `examples/zero_closed.py` | 低层 CAN 台架工具,一个子命令一个操作:`status`、`release`、`goto`、`zero`、`open`、`close`、`enable`、`disable`。 | | `examples/teleop.py` | 主从链路里的其中一端。 | | `examples/trajectory.py` | 手把手教一个动作,存起来,再放一遍。 | diff --git a/examples/api_test.py b/examples/api_test.py new file mode 100644 index 0000000..8c163bc --- /dev/null +++ b/examples/api_test.py @@ -0,0 +1,357 @@ +#!/usr/bin/env python3 +"""Interactive smoke test for the common LiteGrip SDK calls. + +This is the bench script to reach for when you want to exercise the high-level +API one call at a time — read state, enable, open, close, goto, grasp, release, +stop, disable — instead of running one of the focused tests. It prints what +each call returned and the state it ended in. + +**Read-only by default.** ``--sequence read`` (the default) only connects, +loads the calibration and prints a status frame. Any mode that enables or +moves the jaws requires ``--yes``:: + + # What does the motor report right now? + python3 examples/api_test.py --sequence read + + # Open -> goto mid-travel -> grasp 5 N / 2 s -> release 10 mm. + python3 examples/api_test.py --sequence basic --yes + + # The same calls, one menu entry at a time. + python3 examples/api_test.py --sequence menu --yes + +Safety: + +* ``close()`` is a position move to the calibrated closed stop, not a force + limit. Use ``grasp()`` to close on an object. +* ``stop()`` sends zero torque but leaves the motor enabled; ``disable()`` is + what makes the jaws hand-movable. +* Keep a hand on the power switch. The script disables the motor on exit, so + anything held will fall. + +The script calls the SDK exactly as the menu says and interposes no guard of +its own. ``grasp(hold_s>0)`` returns with ``released=False`` and leaves the +last hold torque on the bus until ``release()`` or ``disable()``; a position +move issued before that starts against a drive that is still pressing. +""" + +from __future__ import annotations + +import argparse +import os +import sys +import time +from typing import Any + +# Like the other examples: import the SDK straight out of the checkout, so the +# script works without `pip install -e .`. Inserted first, so the checkout wins +# over an installed copy — running it exercises the code next to it. +_SRC = os.path.join(os.path.dirname(os.path.dirname(os.path.abspath(__file__))), "src") +if _SRC not in sys.path: + sys.path.insert(0, _SRC) + +from litegrip import LiteGrip # noqa: E402 +from litegrip.can import MotorType # noqa: E402 + + +def _hex(value: str) -> int: + """Parse an int written as 0x08, 8, or 0o10 on the command line.""" + return int(value, 0) + + +def build_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser( + description="Interactive smoke test for the high-level LiteGrip API. " + "Read-only unless --yes is given.", + formatter_class=argparse.ArgumentDefaultsHelpFormatter, + ) + parser.add_argument("--yes", action="store_true", + help="allow enable/motion; without it this script is " + "read-only") + parser.add_argument("--sequence", choices=("read", "basic", "menu"), + default="read", + help="read=status only; basic=open/goto/grasp/release; " + "menu=one call at a time") + parser.add_argument("--channel", default="can0", + help="CAN interface (default: can0)") + parser.add_argument("--can-id", type=_hex, default=0x08, + help="motor CAN ID (default: 0x08)") + parser.add_argument("--mst-id", type=_hex, default=None, + help="master/status ID; default auto-detect") + parser.add_argument("--motor", default="DM4310", + choices=[m.name for m in MotorType], + help="motor model (default: DM4310)") + parser.add_argument("--canfd", action="store_true", + help="open the transport in CAN-FD mode") + parser.add_argument("--calib", default=None, + help="explicit calibration JSON. Without it the " + "factory calibration is used: a file this machine " + "measured is never read unless you name it here " + "or in LITEGRIP_CALIB") + parser.add_argument("--template", default=None, + help="calibration template name; see litegrip.list_templates()") + parser.add_argument("--speed", type=float, default=20.0, + help="open/close speed in mm/s") + parser.add_argument("--force", type=float, default=5.0, + help="grasp force in N") + parser.add_argument("--hold-s", type=float, default=2.0, + help="grasp hold time in s; 0 means hold forever") + parser.add_argument("--goto-mm", type=float, default=None, + help="basic mode's goto target; default is mid-travel") + parser.add_argument("--release-mm", type=float, default=10.0, + help="release clearance in mm") + return parser + + +def _fresh_state(g: Any) -> Any: + """Ask for a status frame; retry a few times.""" + last = None + for _ in range(3): + g.refresh_status() + last = g.get_state(wait=True) + if last.fresh: + return last + time.sleep(0.05) + return last + + +def _print_config(g: Any) -> None: + cfg = g.config + travel = abs(cfg.pos_open_rad - cfg.pos_closed_rad) * cfg.rad_to_mm + mst = "auto" if g.mst_id is None else f"0x{g.mst_id:02X}" + print(f"链路: {g.channel} CAN ID=0x{g.can_id:02X} MST ID={mst}") + print(f"校准: calibrated={cfg.calibrated} " + f"pos_closed_rad={cfg.pos_closed_rad:+.5f} " + f"pos_open_rad={cfg.pos_open_rad:+.5f}") + print(f" 行程~{travel:.1f} mm " + f"rad_to_mm={cfg.rad_to_mm:.3f} " + f"work_stroke_mm={cfg.work_stroke_mm:.1f} mm") + + +def _print_state(label: str, state: Any) -> None: + age = "None" if state.data_age_s is None else f"{state.data_age_s:.3f}s" + print(f"{label}: position_rad={state.position_rad:+.5f} " + f"({state.position_mm:+.2f} mm) " + f"velocity_rad_s={state.velocity_rad_s:+.3f} " + f"torque_nm={state.torque_nm:+.3f} " + f"force_n={state.force_n:+.2f} " + f"error_code=0x{state.error_code:X} " + f"fresh={state.fresh} data_age_s={age}") + + +def _print_move(label: str, result: Any) -> None: + print(f"{label}: ok={result.ok} reached={result.reached} " + f"stalled={result.stalled} protected={result.protected} " + f"steps={result.steps}") + _print_state(" 结束", result.state) + + +def _print_grasp(label: str, result: Any) -> None: + print(f"{label}: ok={result.ok} reached={result.reached} " + f"stalled={result.stalled} no_bite={result.no_bite} " + f"travel_guard={result.travel_guard} cycles={result.cycles} " + f"force={result.force_n:.1f} N released={result.released}") + _print_state(" 结束", result.state) + + +def _print_release(label: str, result: Any) -> None: + print(f"{label}: ok={result.ok} opened_mm={result.opened_mm:.1f} " + f"aborted={result.aborted}") + _print_state(" 结束", result.state) + + +def _open_gripper(args: argparse.Namespace, *, disable_on_exit: bool) -> Any: + g = LiteGrip( + channel=args.channel, + can_id=args.can_id, + mst_id=args.mst_id, + canfd_mode=args.canfd or None, + motor_type=MotorType[args.motor], + disable_on_disconnect=disable_on_exit, + ) + # Calibration is a file read, so it happens before connect: the file may + # carry can_id/mst_id, and connect() is what puts those on the wire. + if not g.load_calibration(path=args.calib, template=args.template): + raise SystemExit("错误:没有读到任何校准文件 —— 请传入 " + "--calib 或 --template") + if not g.config.calibrated: + raise SystemExit("错误:校准文件里 calibrated=false;运动层不会按" + "占位限位移动。") + g.connect() + _print_config(g) + return g + + +def do_read(args: argparse.Namespace) -> int: + g = _open_gripper(args, disable_on_exit=False) + try: + state = _fresh_state(g) + _print_state("状态", state) + if not state.fresh: + print("没有新的状态帧:请检查 can0、CAN ID/MST ID 和电机供电。") + return 1 + return 0 + finally: + g.disconnect() + + +def _enable(g: Any) -> bool: + result = g.enable() + print(f"enable: ok={result.ok} tries={result.tries}") + _print_state(" 结束", result.state) + return bool(result.ok) + + +def do_basic(args: argparse.Namespace) -> int: + print("安全:手放在电源开关旁;保持夹爪周围无人。") + g = _open_gripper(args, disable_on_exit=True) + try: + state = _fresh_state(g) + _print_state("初始", state) + if not state.fresh: + print("没有新的状态帧;停止。") + return 1 + if not _enable(g): + return 1 + + ok = True + result = g.open(speed_mm_s=args.speed) + _print_move("open", result) + ok = ok and bool(result.ok) + + cfg = g.config + travel = abs(cfg.pos_open_rad - cfg.pos_closed_rad) * cfg.rad_to_mm + target = args.goto_mm if args.goto_mm is not None else travel / 2.0 + current = g.get_state(wait=False).position_mm + duration = max(0.5, abs(target - current) / max(args.speed, 1.0)) + result = g.goto(target, duration=duration) + _print_move(f"goto {target:.1f} mm", result) + ok = ok and bool(result.ok) + + result = g.grasp(force_n=args.force, hold_s=args.hold_s) + _print_grasp(f"grasp {args.force:.1f} N / {args.hold_s:.1f} s", result) + ok = ok and bool(result.ok) + + result = g.release(clearance_mm=args.release_mm) + _print_release(f"release {args.release_mm:.1f} mm", result) + ok = ok and bool(result.ok) + return 0 if ok else 1 + finally: + try: + g.stop() + except Exception: + pass + try: + g.disable() + except Exception: + pass + g.disconnect() + + +def menu(args: argparse.Namespace) -> int: + print("安全:手放在电源开关旁;保持夹爪周围无人。") + g = _open_gripper(args, disable_on_exit=True) + try: + while True: + print("\n" + "=" * 56) + print("1) get_state() 2) enable() 3) open()") + print("4) close() 5) goto() 6) grasp()") + print("7) release() 8) stop() 9) disable()") + print("0) 退出") + choice = input("选择: ").strip() + if choice in ("0", "q", "quit"): + break + if choice == "1": + _print_state("状态", _fresh_state(g)) + continue + if choice == "2": + _enable(g) + continue + if choice == "9": + print(f"disable: ok={g.disable()}") + continue + if choice not in ("3", "4", "5", "6", "7", "8"): + print("无效的选项。") + continue + if not g.is_enabled: + print("电机未使能;请先选择 2 (enable())。") + continue + + # 动作前主动要一帧状态。空闲总线上 SDK 的 get_state() 只回缓存; + # 夹爪若被手扳过,open/close/goto 会从旧位置规划斜坡。这是刷新, + # 不是护栏。 + _fresh_state(g) + + if choice == "3": + _print_move("open", g.open(speed_mm_s=args.speed)) + elif choice == "4": + print("注意:close() 只做位置移动;要夹住物体请用 grasp()。") + _print_move("close", g.close(speed_mm_s=args.speed)) + elif choice == "5": + try: + target = float(input("目标位置 (mm): ").strip()) + except ValueError: + print("不是数字。") + continue + current = g.get_state(wait=False).position_mm + duration = max(0.5, abs(target - current) / max(args.speed, 1.0)) + _print_move(f"goto {target:.1f} mm", + g.goto(target, duration=duration)) + elif choice == "6": + try: + force = float(input(f"力 (N) [{args.force:g}]: ").strip() + or args.force) + hold = float(input(f"保持时间 (s) [{args.hold_s:g}]: ").strip() + or args.hold_s) + except ValueError: + print("不是数字。") + continue + result = g.grasp(force_n=force, hold_s=hold) + _print_grasp(f"grasp {force:.1f} N / {hold:.1f} s", result) + elif choice == "7": + try: + clearance = float( + input(f"释放间隙 (mm) [{args.release_mm:g}]: ").strip() + or args.release_mm) + except ValueError: + print("不是数字。") + continue + result = g.release(clearance_mm=clearance) + _print_release(f"release {clearance:.1f} mm", result) + elif choice == "8": + g.stop() + print("stop() 已执行:零力矩,电机仍保持使能。") + return 0 + finally: + try: + g.stop() + except Exception: + pass + try: + g.disable() + except Exception: + pass + g.disconnect() + + +def main(argv=None) -> int: + args = build_parser().parse_args(argv) + if args.sequence != "read" and not args.yes: + print("未加 --yes,拒绝使能或运动;只读连接请用 --sequence read。", + file=sys.stderr) + return 2 + try: + if args.sequence == "read": + return do_read(args) + if args.sequence == "basic": + return do_basic(args) + return menu(args) + except KeyboardInterrupt: + print("\n已中断。") + return 130 + except Exception as exc: # noqa: BLE001 - report the real reason + print(f"\n失败:{type(exc).__name__}: {exc}") + return 1 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/src/litegrip/actions.py b/src/litegrip/actions.py index 5c1ddc7..b151fd5 100644 --- a/src/litegrip/actions.py +++ b/src/litegrip/actions.py @@ -46,6 +46,11 @@ 闭合侧有约 0.010 rad 的机械死区,慢速粘滑时单点忽大忽小。斜坡走完的保压段 不判(那时夹爪本来就该不动)。 +带设定力的接近段还多一条**真停**条件:窗口平均速度低于 +:attr:`MotionConfig.stall_speed_mm_s` 才算停住。这一段的实际速度本来就可能被 +力矩预算和摩擦压到指令速度的 20% 以下,只按比例判会把「慢」当成「顶住」, +把纯力矩保力段接到还在走的夹爪上。 + 保力段(``grasp`` 的第二段)反过来:只下发前馈力矩,不给位置/速度增益。 力控要的是力,带增益就会随夹爪的位移衰减,详见 :meth:`GripperActions._hold_force`。 @@ -133,6 +138,12 @@ class MotionConfig: stall_cycles: int = 5 # 窗口采样点数 stall_ratio: float = 0.2 # 窗口净位移 / 本该走的距离 stall_delta: float = 0.0015 # 阈值下限 rad + # 带设定力接近段判「真停」的速度下限 mm/s。这一段是位置帧,但预算和摩擦 + # 会把实际速度压到指令的 20% 以下 —— 那不是接触。窗口平均速度必须低于 + # 这个值才允许交接给纯力矩保力段。2026-11-18 台架:5 N 接近段约 8 mm/s, + # 0.25 s 只走 2 mm,被 20% 阈值判成堵转;保力段随后把它加速到 7 rad/s, + # 从 78 mm 冲到 26 mm 的工件上。 + stall_speed_mm_s: float = 2.0 # ── 力矩 / 指令上限 ──────────────────────────────────────────────── max_lead_mm: float = 4.0 # 行进段领先上限 mm(≈ kp × 上限) @@ -822,7 +833,9 @@ def grasp( 它才一路走到底。 **交接只在「到位或顶上东西」时发生**(``reached or stalled``)。这两条 - 在真机上是同一件事的两面:夹爪都停着。这不是保守,是唯一的正确条件 —— + 在真机上是同一件事的两面:夹爪都停着。带设定力的接近段里,「顶上」还要 + 过一条真停条件:窗口平均速度低于 :attr:`MotionConfig.stall_speed_mm_s` + —— 慢速蠕动不算接触(见该字段)。这不是保守,是唯一的正确条件 —— 保力段是纯力矩源,帧里没有位置项也没有速度项,把它接到一个**还在自由 行程里**的夹爪上,位置环一撤它就只有加速这一条路(2026-10-13 实测: 0.82 rad/s → 5.12 rad/s,312 mm/s,且到 ``hold_s`` 用完仍在加速)。 @@ -1343,7 +1356,7 @@ def _do_release(self, req: _ReleaseRequest) -> ReleaseResult: # 是一次位置运动,方向来自标定,未标定时拒绝。 if not req.abort: _check_calibrated(gcfg) - st = g.get_state() + st = self._fresh_state() pos = st.position_rad if req.abort: @@ -1464,6 +1477,40 @@ def _release(self, pos: float) -> int: self._emit(pos, 0.0, 0.0, kp=0.0, kd=0.0) return frames + def _fresh_state(self) -> GripperState: + """Read a status snapshot, asking the motor to speak if the bus is idle. + + A DM motor answers control frames; when nothing is being sent it says + nothing, and ``get_state(wait=True)`` returns the cache after ~50 ms. + Planning a ramp from that cache is how a jaw that was moved while the + bus was idle made the first frame demand the whole error: on 2026-11-18 + the cache said 78 mm, the jaws were at 26 mm, and ``open()`` left at + 23 rad/s out of a 20 mm/s move. + + ``refresh_status()`` (0xCC) is the request that makes an idle motor + answer. If the bus stays silent and the cache is stale, refuse to plan + motion from an unknown position instead of guessing. + """ + if not self._g.refresh_status(): + st = self._g.get_state(wait=False) + if st.is_stale(): + raise CommError( + "状态帧请求发不出去,缓存位置也已过期(age=%s)—— 拒绝" + "从过期位置规划运动;检查 CAN 链路和电机供电。" + % ("无" if st.data_age_s is None + else "%.1f s" % st.data_age_s)) + return st + st = self._g.get_state() + if st.fresh: + return st + st = self._g.get_state(wait=False) + if st.is_stale(): + raise CommError( + "电机没有回应状态帧请求,缓存位置已过期(age=%s)—— 拒绝" + "从过期位置规划运动。" % ("无" if st.data_age_s is None + else "%.1f s" % st.data_age_s)) + return st + def _move_to_limit( self, toward: str, @@ -1552,7 +1599,7 @@ def _move_to_limit( target, limit, _margin_rad, _travel = limit_target( gcfg, toward, cfg.margin) - before = g.get_state() + before = self._fresh_state() dist_rad = target - before.position_rad dist_mm = abs(dist_rad) * gcfg.rad_to_mm sign = 1.0 if dist_rad >= 0 else -1.0 @@ -1592,11 +1639,15 @@ def _move_to_limit( win_s = win * sample_every * interval win_expect_rad = speed_rad_s * win_s win_thresh_rad = max(cfg.stall_delta, cfg.stall_ratio * win_expect_rad) + # 带设定力接近段的“真停”下限:窗口平均速度低于它才算停住。 + stall_speed_rad_s = cfg.stall_speed_mm_s / gcfg.rad_to_mm # 帧数上界。不带力的移动按排程走,排程跑完就是它的终止条件之一;带设定 # 力的接近段不行 —— 领先被预算压小(见 force_approach_terms),夹爪追不 # 追得上排程由摩擦和读数延迟定,``ramp_steps`` 到了它可能还差几十毫米。 # 所以这一段的帧数只是兜底,上界取「按仍然算进展的最慢速度走完全程要几 - # 帧」:比这更慢时堵转判据必然已经在 win_s 内触发,这个上界到不了。 + # 帧」。比这根速度还慢、但窗口平均又高于 stall_speed_mm_s 的蠕动会走到 + # 这个上界,按「既没到位也没堵转」拒夹;只有低于 stall_speed_mm_s 的 + # 才算真停、才允许交接。 ceiling = total_steps if force_n is not None: floor_rad_s = win_thresh_rad / win_s if win_s > 0.0 else 0.0 @@ -1737,9 +1788,16 @@ def _move_to_limit( # 夹爪到了那里就该顶着不动 —— 那里的「不动」正是我们要的堵转。而且 # 越位量(5% 行程)往往短于一个采样窗口,只在斜坡段判会永远判不到, # 动作得白跑完保压段。 + # 带设定力的接近段:窗口位移小还不够 —— 实际速度被预算/摩擦压到 + # 指令的 20% 以下是常态,那不是停住。再看一眼窗口平均速度,低于 + # stall_speed_mm_s 才算真停,才允许交接给纯力矩保力段。 + moving = (force_n is not None and win_delta is not None + and win_s > 0.0 + and win_delta / win_s > stall_speed_rad_s) if (judge_stall and win_delta is not None and (press or in_ramp) - and win_delta < win_thresh_rad): + and win_delta < win_thresh_rad + and not moving): # 「没动」不等于「顶到工件」:带设定力时,从起点起就没挪过位的 # 停住也可能是这一段压不出脱离静摩擦的力矩。这一条不能当接触, # 否则交接给纯力矩的保力段就是一次带力冲程(见 @@ -1763,8 +1821,9 @@ def _move_to_limit( else: stalled = True log.info("堵转:最近 %d 次采样(%.2f s)净位移仅 %.5f rad " - "(< %.5f),停在 %+.5f rad", - win, win_s, win_delta, win_thresh_rad, pos) + "(< %.5f,平均 %.2f mm/s),停在 %+.5f rad", + win, win_s, win_delta, win_thresh_rad, + win_delta / win_s * gcfg.rad_to_mm, pos) break # 行进段堵转力矩保护(≈7 N):与位置窗口判据**互补** —— 位置窗口对 @@ -1880,7 +1939,7 @@ def _hold_force( # 的片里就成了 4 N 一级的台阶,不是爬升。 step_nm = cfg.force_ramp_n_s * cfg.frame_interval * UnitConversion.N_TO_NM cycles = 0 - st = g.get_state() + st = self._fresh_state() pos = st.position_rad # 进入保力:从飞行中的力矩接着爬,交接才连续。飞行力矩已经越过设定值时 # 直接从设定值起步(那一步是往下走到设定值,不是往上,不构成冲击)。 @@ -1916,7 +1975,7 @@ def _hold_force( self._emit(pos, 0.0, tau_cmd, kp=0.0, kd=0.0) cycles += 1 - st = g.get_state() + st = self._fresh_state() if st.error_code != 1: log.warning("保力中断:状态帧 err=%d" "(1=使能中;0=被禁用;其他=故障)", st.error_code) diff --git a/src/litegrip/can/controller.py b/src/litegrip/can/controller.py index 4dfd3f2..41254e4 100644 --- a/src/litegrip/can/controller.py +++ b/src/litegrip/can/controller.py @@ -19,7 +19,7 @@ from typing import Dict, Optional from ..exceptions import CANTimeoutError -from .transport import CanTransport +from .transport import CanFrame, CanTransport from .protocol import ( BROADCAST_ID, ControlMode, @@ -241,16 +241,39 @@ def save_params(self, motor: MotorState) -> None: # ── polling ────────────────────────────────────────────────────────── def poll(self, timeout_s: float = 0.0) -> Optional[MotorState]: - """Poll for one CAN frame. If it's a status frame for a registered - motor, decode it and update that MotorState. + """Read every status frame already queued; the newest state wins. + + The motor answers each control frame with a status frame. A caller + that sends a burst (a force-hold slice, a ramp step) and polls once per + burst leaves the rest of the answers in the socket buffer; reading one + frame then returns an *old* position, and the age is stamped when the + frame is read — so the stale reading looks fresh. On 2026-11-18 that + made a position ramp start from the pose the jaws had at the start of a + hold: the first frame demanded ~0.7 rad of error and the jaws left at + 23 rad/s out of a 20 mm/s move. + + ``timeout_s`` only applies to the wait for the first frame; the drain + after it is non-blocking. Parameter responses and foreign frames are + discarded without stopping the drain. Returns: - The MotorState that was updated, or None if no relevant frame arrived. + The MotorState updated by the newest relevant frame, or None if no + relevant frame arrived. """ - frame = self._transport.recv(timeout_s=timeout_s) - if frame is None: - return None + updated: Optional[MotorState] = None + first = True + while True: + frame = self._transport.recv( + timeout_s=timeout_s if first else 0.0) + first = False + if frame is None: + return updated + motor = self._apply_status_frame(frame) + if motor is not None: + updated = motor + def _apply_status_frame(self, frame: CanFrame) -> Optional[MotorState]: + """Decode one received frame into its motor state, if it is one.""" # Match frame.can_id against registered motors' mst_id motor = self._motors.get(frame.can_id) if motor is None: diff --git a/tests/fake_can.py b/tests/fake_can.py index bdb3d03..afd9930 100644 --- a/tests/fake_can.py +++ b/tests/fake_can.py @@ -82,10 +82,14 @@ def __init__(self, pos: float = 0.0, block_rad: Optional[float] = None, yield_rad_s: float = 0.0, yield_tau_nm: float = 0.0, creep_rad_s_per_nm: float = 0.0, report_lag_ticks: int = 0, - breakaway_tau_nm: float = 0.0): + breakaway_tau_nm: float = 0.0, + speed_cap_rad_s: Optional[float] = None): # 静摩擦的脱离力矩 Nm:站在原地的夹爪要帧里力矩过这个门槛才动。 # 0 = 没有静摩擦(默认,别的用例逐位不变)。 self.breakaway_tau_nm = breakaway_tau_nm + # 速度上限 rad/s。设置后模拟带力接近段被摩擦/预算压慢的工况: + # 指令 50 mm/s,实际只能爬。None = 只有 VMAX。 + self.speed_cap_rad_s = speed_cap_rad_s # 读数延迟(帧)。真机上 ``get_state(wait=False)`` 拿的是缓存读数,可能 # 落后几帧;而力控接近段的指令是 ``min(排程, 读数 + 领先上限)``,领先只 # 有零点几毫米时,``速度 × 延迟`` 就把它吃光了 —— 夹爪于是推不动排程。 @@ -166,7 +170,9 @@ def step(self, q: float, kp: float, dq: float, tau_ff: float, dt: float) -> None if (self.breakaway_tau_nm > 0.0 and self.vel == 0.0 and abs(kp * (q - self.pos) + tau_ff) < self.breakaway_tau_nm): v = 0.0 - v = max(-self.VMAX, min(self.VMAX, v)) + vmax = (self.VMAX if self.speed_cap_rad_s is None + else min(self.VMAX, self.speed_cap_rad_s)) + v = max(-vmax, min(vmax, v)) new = self.pos + v * dt if self.block_rad is not None: if self._block_dir > 0: @@ -206,7 +212,9 @@ def __init__(self, pos: float = 0.0, block_rad: Optional[float] = None, yield_rad_s: float = 0.0, yield_tau_nm: float = 0.0, creep_rad_s_per_nm: float = 0.0, report_lag_ticks: int = 0, - breakaway_tau_nm: float = 0.0): + breakaway_tau_nm: float = 0.0, + speed_cap_rad_s: Optional[float] = None, + freeze_position_while_idle: bool = False): self.motor = FakeMotor(pos=pos, block_rad=block_rad, sticky_rad=sticky_rad, err=err, limit_lo=limit_lo, limit_hi=limit_hi, @@ -214,7 +222,13 @@ def __init__(self, pos: float = 0.0, block_rad: Optional[float] = None, yield_tau_nm=yield_tau_nm, creep_rad_s_per_nm=creep_rad_s_per_nm, report_lag_ticks=report_lag_ticks, - breakaway_tau_nm=breakaway_tau_nm) + breakaway_tau_nm=breakaway_tau_nm, + speed_cap_rad_s=speed_cap_rad_s) + #: True = 没有帧到达时 ``get_position`` 不露真值,而是停在最后一次 + #: 收到的读数。真机就是这样:读数只在状态帧里更新,总线空闲时不会 + #: 自己变。默认 False,别的用例不受影响。 + self._freeze_position = freeze_position_while_idle + self._position_cache = self.motor.reported_pos() self.frames: List[Frame] = [] self.initialize_results = list(initialize_results or []) self.initialize_calls = 0 @@ -299,6 +313,8 @@ def _frame_available(self) -> bool: self.refresh_pending = False self.last_update = time.monotonic() self.last_frame_time = time.time() + if self._freeze_position: + self._position_cache = self.motor.reported_pos() return True return False @@ -310,6 +326,8 @@ def refresh_status(self) -> bool: return self.refresh_ok def get_position(self) -> float: + if self._freeze_position: + return self._position_cache return self.motor.reported_pos() def get_velocity(self) -> float: @@ -384,6 +402,8 @@ def make_gripper( creep_rad_s_per_nm: float = 0.0, report_lag_ticks: int = 0, breakaway_tau_nm: float = 0.0, + speed_cap_rad_s: Optional[float] = None, + freeze_position_while_idle: bool = False, ): """造一个「已连接、已使能、已标定」的 LiteGrip,底层换成 :class:`FakeLiteGripCAN`。 @@ -438,7 +458,9 @@ def make_gripper( yield_tau_nm=yield_tau_nm, creep_rad_s_per_nm=creep_rad_s_per_nm, report_lag_ticks=report_lag_ticks, - breakaway_tau_nm=breakaway_tau_nm) + breakaway_tau_nm=breakaway_tau_nm, + speed_cap_rad_s=speed_cap_rad_s, + freeze_position_while_idle=freeze_position_while_idle) g._can = fake g._connected = True g._enabled = True diff --git a/tests/test_actions.py b/tests/test_actions.py index e2028e3..3fd8ae0 100644 --- a/tests/test_actions.py +++ b/tests/test_actions.py @@ -989,6 +989,73 @@ def test_neither_reached_nor_stalled_is_not_handed_over(self): self.assertTrue(any(f.kp == 0.0 for f in fake.frames), "没有撤力帧") +class TestForceApproachNeedsATrueStop(unittest.TestCase): + """2026-11-18 台架:5 N 接近段实际只有约 8 mm/s,0.25 s 走 2 mm。 + + 20% 的窗口阈值把「慢」判成「顶上东西」,保力段随后在自由行程里把夹爪 + 加速到 7 rad/s,从 78 mm 冲到 26 mm 的工件上。交接前必须看到**真停**: + 窗口平均速度低于 ``MotionConfig.stall_speed_mm_s``。 + """ + + def test_a_slow_creep_is_not_handed_over_mid_travel(self): + object_rad = (POS_OPEN_RAD + POS_CLOSED_RAD) / 2.0 + motion = MotionConfig(sleep_fn=lambda _: None, + monotonic_fn=tick_clock(0.1)) + crawl_rad_s = 0.109 # ≈ 8 mm/s at RAD_TO_MM + g, fake = make_gripper(block_rad=object_rad, stops=True, + motion=motion, speed_cap_rad_s=crawl_rad_s) + + g.grasp(force_n=5.0, hold_s=0.2) + + holds = [f for f in fake.frames if f.tau_ff != 0.0] + self.assertTrue(holds, "没有进入保力段") + first = holds[0].pos_after + self.assertLess( + abs(first - object_rad), g.motion_config.reach_tol, + "保力段在离工件 %.2f mm 处接手了 —— 又是自由行程里交接" % + (abs(first - object_rad) * RAD_TO_MM)) + + def test_a_slow_creep_without_contact_never_enters_the_hold(self): + """没有工件、只是被摩擦压在 8 mm/s:走到帧数上界按拒夹收尾。 + + 这条钉的是「不交接」本身,而不只是「交接点提前」:慢速蠕动既没到位 + 也没真停,保力段一帧都不许发 —— 它是纯力矩源,接过去只会继续加速。 + """ + motion = MotionConfig(sleep_fn=lambda _: None, + monotonic_fn=tick_clock(0.1)) + g, fake = make_gripper(stops=True, motion=motion, + speed_cap_rad_s=0.109) # ≈ 8 mm/s + + res = g.grasp(force_n=5.0, hold_s=0.2) + + self.assertFalse(res.ok, res) + self.assertFalse(res.stalled, res) + self.assertFalse(res.reached, res) + self.assertEqual(res.cycles, 0) + self.assertEqual([f for f in fake.frames if f.tau_ff != 0.0], [], + "慢速蠕动被交给了纯力矩的保力段") + + +class TestMotionRefreshesAnIdleBus(unittest.TestCase): + """空闲总线上的读数只在状态帧里更新;动作不能从手扳之前的缓存起跳。""" + + def test_open_does_not_start_from_a_position_read_before_the_bus_went_idle(self): + g, fake = make_gripper(stops=True, freeze_position_while_idle=True) + g.get_state() # 缓存停在张开侧 + fake.delivers = False + actual = POS_CLOSED_RAD - 0.4 # 总线空闲时被手扳到中间 + fake.motor.pos = actual + + g.open() + + first = fake.frames[0] + self.assertLess( + abs(first.q - actual), 0.01, + "open 从缓存位置起跳 %.1f mm(实际在 %.1f mm)" % + (abs(first.q - actual) * RAD_TO_MM, + abs(actual - fake.motor.pos) * RAD_TO_MM)) + + class TestRelease(unittest.TestCase): """松开:跨线程结束 ``grasp()``、从交接点张余量、急停、不重复张开。 diff --git a/tests/test_can_poll_drain.py b/tests/test_can_poll_drain.py new file mode 100644 index 0000000..71267db --- /dev/null +++ b/tests/test_can_poll_drain.py @@ -0,0 +1,114 @@ +"""``MotorController.poll`` must drain the RX queue; the newest frame wins. + +The motor answers every control frame with a status frame. A caller that +sends a burst (a force-hold slice, a ramp step) and polls once per burst leaves +the answers queued; reading one frame returns an old position, and the age is +stamped when it is read, so the stale reading looks fresh. On 2026-11-18 the +ramp after a hold started from the hold's first pose (78 mm) while the jaws +were at 26 mm — the first frame demanded ~0.7 rad, and the drive left at +23 rad/s out of a 20 mm/s move. +""" + +from __future__ import annotations + +import unittest + +import _sdkpath # noqa: F401 +from litegrip.can.controller import MotorController +from litegrip.can.protocol import float_to_uint +from litegrip.can.transport import CanFrame + +MST_ID = 0x18 +CAN_ID = 0x08 + + +def status_frame(position_rad: float, err: int = 1) -> CanFrame: + """Build one DM status frame (q, dq=0, tau=0).""" + q_uint = float_to_uint(position_rad, -12.5, 12.5, 16) + dq_uint = float_to_uint(0.0, -30.0, 30.0, 12) + tau_uint = float_to_uint(0.0, -10.0, 10.0, 12) + data = bytes([ + ((err & 0x0F) << 4) | (CAN_ID & 0x0F), + (q_uint >> 8) & 0xFF, + q_uint & 0xFF, + (dq_uint >> 4) & 0xFF, + ((dq_uint & 0x0F) << 4) | ((tau_uint >> 8) & 0x0F), + tau_uint & 0xFF, + 30, 30, + ]) + return CanFrame(can_id=MST_ID, data=data) + + +class QueueTransport: + """A transport that hands out pre-queued frames, then goes quiet.""" + + def __init__(self, frames): + self._frames = list(frames) + self.sent = [] + + def recv(self, timeout_s: float = 0.0): + return self._frames.pop(0) if self._frames else None + + def send(self, can_id, data, timeout=0.1): + self.sent.append((can_id, data)) + + def send_multi(self, can_id, data, count=1, interval_s=0.002): + for _ in range(count): + self.send(can_id, data) + + def set_id_filter(self, ids): + pass + + +class TestPollDrainsTheQueue(unittest.TestCase): + + def make_controller(self, frames): + transport = QueueTransport(frames) + controller = MotorController(transport) + motor = controller.add_motor(can_id=CAN_ID, mst_id=MST_ID) + return controller, motor + + def test_the_newest_queued_frame_wins(self): + controller, motor = self.make_controller([ + status_frame(-1.0), + status_frame(-0.5), + status_frame(-0.2), + ]) + + updated = controller.poll() + + self.assertIs(updated, motor) + self.assertEqual(motor.rx_count, 3, "队列没有被排空") + self.assertAlmostEqual(motor.position, -0.2, places=3, + msg="读到的还是队列里最旧的那一帧") + + def test_foreign_frames_between_status_frames_do_not_stop_the_drain(self): + foreign = CanFrame(can_id=0x123, data=bytes(8)) + controller, motor = self.make_controller([ + status_frame(-1.0), + foreign, + status_frame(-0.3), + ]) + + controller.poll() + + self.assertEqual(motor.rx_count, 2) + self.assertAlmostEqual(motor.position, -0.3, places=3) + + def test_a_single_frame_is_still_reported(self): + controller, motor = self.make_controller([status_frame(0.25)]) + + updated = controller.poll() + + self.assertIs(updated, motor) + self.assertEqual(motor.rx_count, 1) + self.assertAlmostEqual(motor.position, 0.25, places=3) + + def test_an_empty_queue_returns_none(self): + controller, _ = self.make_controller([]) + + self.assertIsNone(controller.poll()) + + +if __name__ == "__main__": + unittest.main()