From d8cf393ee9302a732ef769f8d9c2026878d79838 Mon Sep 17 00:00:00 2001 From: cao-xiao-hao <127832643+cao-xiao-hao@users.noreply.github.com> Date: Thu, 29 Oct 2026 16:42:43 +0800 Subject: [PATCH 1/3] feat: add thread-safe release api for ending a grasp --- src/litegrip/__init__.py | 2 + src/litegrip/actions.py | 464 ++++++++++++++++++++++++++++++++++++--- src/litegrip/gripper.py | 80 ++++++- tests/test_actions.py | 236 +++++++++++++++++++- 4 files changed, 745 insertions(+), 37 deletions(-) diff --git a/src/litegrip/__init__.py b/src/litegrip/__init__.py index fa22171..461fe5d 100644 --- a/src/litegrip/__init__.py +++ b/src/litegrip/__init__.py @@ -75,6 +75,7 @@ def _detect_version(dist_name: str = "litegrip-python") -> str: MoveProgress, MoveResult, GraspResult, + ReleaseResult, HoldResult, EnableResult, WriteZeroResult, @@ -210,6 +211,7 @@ def __dir__(): "MoveProgress", "MoveResult", "GraspResult", + "ReleaseResult", "HoldResult", "EnableResult", "WriteZeroResult", diff --git a/src/litegrip/actions.py b/src/litegrip/actions.py index 3780a79..d62b007 100644 --- a/src/litegrip/actions.py +++ b/src/litegrip/actions.py @@ -64,6 +64,7 @@ import logging import math +import threading import time from dataclasses import dataclass, field from typing import TYPE_CHECKING, Callable, Optional, Tuple @@ -108,6 +109,10 @@ class MotionConfig: sample_interval: float = 0.05 # 20 Hz 堵转采样间隔 s settle_s: float = 0.3 # 斜坡后原地保目标时长 s(不判堵转) reach_tol: float = 0.02 # 到位容差 rad(闭合侧死区 0.0103) + # release() 在夹取进行中等待「夹取线程做完松开」的超时 s。10 s 足够覆盖 + # 默认 50 mm/s 下 10 mm 的张开斜坡(0.2 s)加上交接前保力片的收尾;等不到 + # 就返回 ok=False,不让调用方(往往是 UI 线程或急停按钮)无限期卡住。 + release_wait_s: float = 10.0 # ── open/close 顶限位 ────────────────────────────────────────────── # 这几个数只影响**目标的排程**和接近段的领先:真正下发的指令被 @@ -267,6 +272,10 @@ class MoveResult: # 指令停在 ``实测位置 + 领先上限`` 上,留着它驱动器会按约 6.6 N·m 一直压着。 # 只报「撤了力」,不改变 ``ok``。 released: bool = False + # 这次移动被松开请求(``release()`` / ``request_release()``)从半路叫停。 + # 只有 ``grasp()`` 的带力接近段会检查它:那一段走的不是用户要的终点,而是 + # 夹取的第一步,松开请求意味着「别夹了」。``grasp()`` 据此改走张开。 + release_requested: bool = False def __bool__(self) -> bool: return self.ok @@ -289,6 +298,31 @@ class GraspResult: # 这一路守的是保力段的越位。正常夹取不会置位:夹着工件时夹爪停在工件上, # 空载时顶到真限位就停。置位说明闭合侧没有东西拦着它。 travel_guard: bool = False + # 这次夹取是被 ``release()`` / ``request_release()`` 结束的:夹取线程收到 + # 请求后先完成松开(张开到 ``max(当前, 夹取点) + 余量``),再返回。与 + # ``travel_guard`` 的区别是这不是故障 —— 松开是用户要的正常收尾。 + released: bool = False + + def __bool__(self) -> bool: + return self.ok + + +@dataclass +class ReleaseResult: + """一次松开(:meth:`GripperActions.release`)的结果。 + + ``opened_mm`` 是这一次松开**指令目标**的开口量(mm,从闭合零点算起, + 与 :attr:`GripperState.position_mm` 同一个口径),不是「又张开了多少」; + 夹爪实际停在哪看 ``state.position_mm``。``aborted=True`` 表示这是一次急停 + (``abort=True``):只撤力、不发张开运动,此时 ``opened_mm`` 是撤力那一刻 + 的开口量,``target_rad`` 是撤力那一刻的位置。 + """ + + ok: bool # 松开是否完成(急停与超时为 False) + state: GripperState # 松开结束时的状态 + opened_mm: float # 目标开口量 mm(急停:撤力时的开口量) + target_rad: float # 张开斜坡的目标位置 rad + aborted: bool = False # 这是急停(只撤力,不张开) def __bool__(self) -> bool: return self.ok @@ -305,11 +339,22 @@ class HoldResult: cycles: int # 保力片数 state: Optional[GripperState] # 最后一次状态 guarded: bool = False # 是否被位置护栏中止(详见 GraspResult) + # 保力被松开请求叫停(详见 GraspResult.released)。请求没被消费,仍留在 + # ``_release_req`` 里,由 ``grasp()`` 取走并执行张开/急停。 + release_requested: bool = False def __bool__(self) -> bool: return self.ok +@dataclass(frozen=True) +class _ReleaseRequest: + """一条待执行的松开请求:从夹取线程之外递进来的,只由它自己消费。""" + + clearance_mm: float + abort: bool + + @dataclass class EnableResult: """enable 的结果。""" @@ -649,6 +694,31 @@ def __init__(self, gripper: "LiteGrip", config: Optional[MotionConfig] = None): self._g = gripper self.config = config if config is not None else MotionConfig() + # 松开的跨线程协调。``release()`` 可以从任意线程调用,而帧只能由夹取 + # 线程发(同一根 CAN 上两个线程各发各的,斜坡的领先上限就不再成立)。 + # 所以跨线程时调用方只**记下请求**,夹取线程在每一帧之前查一次;松开 + # 完成后夹取线程记下结果、置 ``_hold_done``,调用方据此返回。 + # + # 这把锁只保护下面这几个字段,**不碰 CAN**:``request_release()`` 在 + # 调用方线程里拿它,夹取线程每 5 ms 拿一次 —— 两边都只做几次赋值。 + # 千万别把 ``_io_lock`` 也套进来(那是帧的锁,见 LiteGrip)。 + self._release_lock = threading.Lock() + # 「没有夹取在跑」时为 set。release() 用它判断有没有夹取可等,也用它 + # 等夹取线程把松开做完。用 Event 而不是 Condition:等待方只关心「完没 + # 完」,不关心完成了几次。 + self._hold_done = threading.Event() + self._hold_done.set() + # 进行中的那条请求(``_ReleaseRequest``)与它的结果。请求只由夹取线程 + # 消费;结果在 ``_hold_done`` 置位之前写入,等待方醒来一定看得到。 + self._release_req: Optional[_ReleaseRequest] = None + self._release_result: Optional[ReleaseResult] = None + # 交接点(带力接近段结束、保力接手处)的实测位置。松开的基准是**夹取 + # 声明的位置**,不是夹爪被力压实后停下的位置:真机上 20 N 会把它往里 + # 多推几毫米,从「停下处」再张 10 mm 就少松了那几毫米 —— 这正是 + # console(litegrip-studio)的 BackOff 要修的那件事。``None`` = 还没 + # 交接(例如接近途中就要求松开)。 + self._grip_rad: Optional[float] = None + # ═══════════════════════════════════════════════════════════════════ # 六个接口 # ═══════════════════════════════════════════════════════════════════ @@ -727,13 +797,15 @@ def grasp( Args: force_n: 夹持力 N,``None`` = 用 :attr:`MotionConfig.force_n`。 按 SDK 近似换算 1 N = 0.1 Nm。 - hold_s: 保力时长 s。``0`` = 一直保到出错、被护栏中止或 Ctrl+C。 + hold_s: 保力时长 s。``0`` = 一直保到出错、被护栏中止、收到 + ``release()`` / ``request_release()`` 的松开请求,或 Ctrl+C。 progress: 进度回调。 Returns: :class:`GraspResult`。夹住工件时 ``stalled=True`` 且 ``reached=False``(压不到空载目标位置是正常的)。闭合段结束时夹爪 - 既没到位也没堵转时 ``ok=False``、``cycles=0`` —— 见上。 + 既没到位也没堵转时 ``ok=False``、``cycles=0`` —— 见上。被松开结束 + 时 ``released=True``:正常松开 ``ok=True``,急停 ``ok=False``。 Raises: CommandError: ``force_n`` / ``hold_s`` 不是有限数。两条都不是 @@ -756,48 +828,238 @@ def grasp( gcfg = self._g.config _check_calibrated(gcfg) - # 闭合段瞄的是**闭合侧标定限位本身**,不是 limit_target 在限位内侧留的 - # 那 5% 行程。留白那几毫米是自由行程:夹爪到了点上、前面什么都没有,保 - # 力段随后只能用纯力矩把它推过去 —— 那是真机上「末端飞快一下」的一半来 - # 源。瞄限位时夹爪是顶在东西上或停在限位上的,交接从静止开始。 - # ``target_rad`` 会强制 press=False,ok 与 reached 的判据都不动。 - move = self._move_to_limit( - "close", cfg.grasp_speed_mm_s, - target_rad=gcfg.pos_closed_rad, - force_n=force, progress=progress) - if not (move.reached or move.stalled): - # 既没到位也没顶上东西 —— 夹爪还在自由行程里,而位置环是此刻唯一 - # 拦着它的东西(kp × 领先 + kd × 速度误差)。保力段的帧里这两项全 - # 没有,交过去就是把设定力矩直接接到一个还在走的夹爪上:真机实测速 - # 度从被速度环限住的 0.82 rad/s 跳到 5.12 rad/s,且到 hold_s 用完 - # 还在加速。宁可如实报失败,也不交。 - self._release(move.state.position_rad) - log.warning("夹取未交接:闭合段结束时夹爪既未到位也未堵转(停在 " - "%+.5f rad,目标 %+.5f rad)—— 它还在自由行程里,而保力" - "段没有位置项也没有速度项,交过去会一路加速。撤力返回。", - move.state.position_rad, move.target_rad) + # 新的一次夹取从干净的状态开始:上一次夹取遗留的松开请求**必须**清掉, + # 否则一个为上一次夹取按下的「松开」会把它自己还没开始的夹取杀掉。 + # 结果也清掉,免得这一次的 release() 拿到上一次的返回值。 + with self._release_lock: + self._release_req = None + self._release_result = None + self._grip_rad = None + self._hold_done.clear() + try: + # 闭合段瞄的是**闭合侧标定限位本身**,不是 limit_target 在限位内侧 + # 留的那 5% 行程。留白那几毫米是自由行程:夹爪到了点上、前面什么 + # 都没有,保力段随后只能用纯力矩把它推过去 —— 那是真机上「末端飞 + # 快一下」的一半来源。瞄限位时夹爪是顶在东西上或停在限位上的,交 + # 接从静止开始。``target_rad`` 会强制 press=False,ok 与 reached 的 + # 判据都不动。 + move = self._move_to_limit( + "close", cfg.grasp_speed_mm_s, + target_rad=gcfg.pos_closed_rad, + force_n=force, progress=progress) + + # 接近途中就要求松开:还没有交接点,基准退化成当前位置。请求在 + # ``_move_to_limit`` 里被看到(带力段每帧查一次),这里取走并执行。 + req = self._take_release_request() + if req is not None: + return self._grasp_ends_with_release(req, move, force, cycles=0) + + if not (move.reached or move.stalled): + # 既没到位也没顶上东西 —— 夹爪还在自由行程里,而位置环是此刻 + # 唯一拦着它的东西(kp × 领先 + kd × 速度误差)。保力段的帧里 + # 这两项全没有,交过去就是把设定力矩直接接到一个还在走的夹爪 + # 上:真机实测速度从被速度环限住的 0.82 rad/s 跳到 5.12 rad/s, + # 且到 hold_s 用完还在加速。宁可如实报失败,也不交。 + self._release(move.state.position_rad) + log.warning("夹取未交接:闭合段结束时夹爪既未到位也未堵转(停在 " + "%+.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, + ) + + # 交接点:带力接近段的终点(夹爪「声明」抓住的位置),不是随后被 + # 20 N 压实后停下的位置。松开的基准取它,见 _do_release。 + # 「记交接点」和「查请求」必须在同一把锁里:否则请求可能正好落在 + # 两者之间 —— 请求已经递进来,而这一段却没看见它。 + with self._release_lock: + if self._release_req is None: + self._grip_rad = move.state.position_rad + req = None + else: + req = self._release_req + self._release_req = None + if req is not None: + return self._grasp_ends_with_release(req, move, force, cycles=0) + + hold = self._hold_force(force, hold_s, progress=progress) + if hold.release_requested: + req = self._take_release_request() + if req is None: # 兜底:请求不该凭空消失 + req = _ReleaseRequest(10.0, False) + return self._grasp_ends_with_release( + req, move, force, cycles=hold.cycles) return GraspResult( - ok=False, + ok=hold.ok, reached=move.reached, stalled=move.stalled, - state=move.state, + state=hold.state if hold.state is not None else move.state, target_rad=move.target_rad, force_n=force, - cycles=0, - travel_guard=False, + cycles=hold.cycles, + travel_guard=hold.guarded, ) - hold = self._hold_force(force, hold_s, progress=progress) + finally: + # 一次夹取结束 = 没有夹取可松开了。两件事在同一把锁里做,等待方 + # 醒来时请求一定已经清掉、结果一定已经写好。 + with self._release_lock: + self._release_req = None + self._grip_rad = None + self._hold_done.set() + + def _grasp_ends_with_release( + self, + req: _ReleaseRequest, + move: MoveResult, + force: float, + *, + cycles: int, + ) -> GraspResult: + """夹取线程执行一条松开请求,并把结果折成 :class:`GraspResult`。 + + 正常松开 ``ok=True``;急停 ``ok=False``。两者都是 ``released=True`` —— + 夹取是被松开结束的,不是故障。结果先记进 ``_release_result`` 再让 + ``grasp()`` 的 ``finally`` 置 ``_hold_done``,等待中的 ``release()`` + 因此一定拿得到它。 + """ + result = self._do_release(req) + with self._release_lock: + self._release_result = result return GraspResult( - ok=hold.ok, + ok=result.ok, reached=move.reached, stalled=move.stalled, - state=hold.state if hold.state is not None else move.state, + state=result.state, target_rad=move.target_rad, force_n=force, - cycles=hold.cycles, - travel_guard=hold.guarded, + cycles=cycles, + travel_guard=False, + released=True, ) + def release( + self, + clearance_mm: float = 10.0, + *, + abort: bool = False, + timeout_s: Optional[float] = None, + ) -> ReleaseResult: + """松开:结束进行中的 ``grasp()`` 保力并张开夹爪。 + + 可以从**任意线程**调用,包括 ``grasp(hold_s=0)`` 还堵在另一个线程里的 + 时候。这条限制不是礼貌,是正确性:帧只能由夹取线程发 —— 两个线程各自 + 往同一根 CAN 上发斜坡,各自的领先上限就都不成立了;而保力段是纯力矩 + 帧,中间插进来的位置帧会把它交接时的连续爬升打断。所以: + + - **有夹取在跑**:本方法只记下请求(参数取第一次那条;``abort=True`` + 会压过排队中的普通松开),然后等夹取线程做完松开。夹取线程在接近段/保力段的**每一帧之前**查请求,5 ms + 内就能看到;它自己完成张开(或急停)并记下结果,本方法醒来返回它。 + 等满 ``timeout_s``(``None`` = :attr:`MotionConfig.release_wait_s`) + 还没等到,返回 ``ok=False`` 并记一条 warning —— 不在这里抢着发帧。 + - **没有夹取在跑**:就在调用方线程里直接做这一次松开。 + + ``abort=True`` 是急停:**只撤力、不发张开运动**,尽快返回。``abort`` + 与普通松开走同一条等待路径,差别只在夹取线程做什么。 + + 连续调用是安全的:同一个夹取里第二次 ``release()`` 不会再多张开一次 + (它等到第一次的结果直接返回);没有夹取时每次都按「当前位置 + 余量」 + 张开,本来就不会抛「没有夹取」。 + + 张开逻辑与 console 的 BackOff 一致:基准是**交接点**(``grasp()`` 里 + ``move.state.position_rad``)而不是力压实后停下的位置,目标是 + ``max(当前, 交接点) + clearance_mm``,再夹到标定张开限位;走的是既有的 + 有界位置斜坡(:meth:`_move_to_limit`),没有阶跃指令。还没交接就要求 + 松开时,基准退化成当前位置。张开速度用 + :attr:`MotionConfig.grasp_speed_mm_s` —— 松开是夹取的收尾动作,与闭合 + 段同一速度;``open()`` 的 ``speed_mm_s`` 默认值相同(都是 50 mm/s)。 + + Args: + clearance_mm: 在基准位置上再张开的余量 mm,必须是非负有限数。 + abort: 急停:只撤力,不张开。 + timeout_s: 等待夹取线程完成的超时 s,``None`` = 配置值。 + + Returns: + :class:`ReleaseResult`。 + + Raises: + CommandError: ``clearance_mm`` 不是非负有限数;没有夹取在跑且配置 + 尚未标定(方向是猜的,不给它发运动)。 + NotInitializedError: 没有夹取在跑、夹爪未连接或未使能 —— 原样抛出 + 底层记下的那一条。 + """ + if timeout_s is not None and not math.isfinite(timeout_s): + raise CommandError( + f"松开等待超时必须是有限秒数:timeout_s={timeout_s!r}。" + "NaN 会让等待的截止时间没有意义。") + req = self._check_release_args(clearance_mm, abort) + with self._release_lock: + active = not self._hold_done.is_set() + if active: + if self._release_req is None: + self._release_req = req + elif req.abort and not self._release_req.abort: + # 急停压过已经排队的一次普通松开:排队中的请求还没执行, + # 换成撤力。反过来不行 —— 已经叫了急停就不要又被一次普通 + # 松开带回张开运动。 + self._release_req = req + else: + # 已经有一条请求在排队:参数取第一条,第二次只是「再催 + # 一次」。两次同时到达也不会张开两次。 + req = self._release_req + + if not active: + return self._do_release(req) + + wait_s = (self.config.release_wait_s + if timeout_s is None else float(timeout_s)) + if wait_s <= 0.0 or not self._hold_done.wait(wait_s): + log.warning("松开等待超时:%.1f s 内夹取线程没有完成松开" + "(abort=%s)—— 返回 ok=False,不在这里抢着发帧。", + wait_s, req.abort) + return self._release_timeout_result(req) + + with self._release_lock: + outcome = self._release_result + leftover = self._release_req + if leftover is not None: + self._release_req = None + if outcome is not None: + return outcome + # 夹取在请求落地前就自然结束了(例如 hold_s 刚好到点):此刻已经没有 + # 夹取可等,就在本线程里把这次松开做掉,而不是把请求吞掉。 + return self._do_release(leftover if leftover is not None else req) + + def request_release(self, clearance_mm: float = 10.0, *, + abort: bool = False) -> bool: + """非阻塞版 :meth:`release`:只记请求,返回「有没有夹取可松开」。 + + 有夹取在跑时置上请求并返回 ``True``;没有就返回 ``False``。它**从不** + 碰 CAN,也不获取 ``_io_lock`` —— 给 UI 线程、信号处理、E-stop 回调这类 + 不能等总线的地方用。实际张开发帧由夹取线程在下一帧之前接手。 + + 重复调用安全:已经有一条请求在排队时,保留第一条的参数;唯一的例外是 + ``abort=True`` 会压过排队中的普通松开(急停优先)。 + + Raises: + CommandError: ``clearance_mm`` 不是非负有限数(与有没有夹取无关)。 + """ + req = self._check_release_args(clearance_mm, abort) + with self._release_lock: + if self._hold_done.is_set(): + return False + if (self._release_req is None + or (req.abort and not self._release_req.abort)): + self._release_req = req + return True + def ramp_to( self, target_rad: float, @@ -954,6 +1216,118 @@ def disable(self) -> bool: # 引擎 # ═══════════════════════════════════════════════════════════════════ + def _check_release_args(self, clearance_mm: float, + abort: bool) -> _ReleaseRequest: + """校验松开参数并折成请求。NaN / 负数在这里拒绝,不是夹到 0。""" + if not math.isfinite(clearance_mm) or clearance_mm < 0.0: + raise CommandError( + f"松开的余量必须是**非负有限数**:clearance_mm=" + f"{clearance_mm!r}。负余量是「再夹进去一点」,不是松开;NaN " + "进了位置算术会变成 NaN 目标。") + return _ReleaseRequest(float(clearance_mm), bool(abort)) + + def _release_pending(self) -> bool: + """有没有一条还没被夹取线程消费的松开请求(只碰松开锁)。""" + with self._release_lock: + return self._release_req is not None + + def _take_release_request(self) -> Optional[_ReleaseRequest]: + """取走并清空待执行的松开请求;没有就返回 ``None``。 + + 只由夹取线程调用(``_hold_force`` / ``_move_to_limit`` 只查不取)。 + """ + with self._release_lock: + req = self._release_req + self._release_req = None + return req + + def _do_release(self, req: _ReleaseRequest) -> ReleaseResult: + """执行一条松开请求:急停只撤力,普通松开走有界斜坡张开。 + + 基准与 console(litegrip-studio)的 BackOff 同一条:从**交接点**再张 + ``clearance_mm``,而不是从力压实后停下的位置。真机上设定力会把夹爪往 + 工件里多推几毫米,从停下处再张 10 mm 就少松了那几毫米 —— console 那 + 个按钮要修的正是这件事。``max(当前, 交接点)`` 是因为夹爪也可能被外力 + (或回弹的工件)推到更外面,那时取更远的那端。还没交接时 + ``_grip_rad is None``,基准就是当前位置。 + + 目标再经 :func:`clamp_to_travel` 夹到标定行程内 —— 也就是最多张到标定 + 张开限位,不会因为余量大就越过去。运动是既有的有界斜坡 + (:meth:`_move_to_limit`,``target_rad`` 语义),不是阶跃指令。 + """ + cfg = self.config + g = self._g + gcfg = g.config + # 急停不要求标定:标定坏了、文件没加载,也拦不住一次撤力。普通松开 + # 是一次位置运动,方向来自标定,未标定时拒绝。 + if not req.abort: + _check_calibrated(gcfg) + st = g.get_state() + pos = st.position_rad + + if req.abort: + # 急停:一帧 ``kp=kd=tau=0`` 就够停下力矩,随后立刻返回。不发张开 + # 运动 —— 调用方按的是「别动了」,不是「张到某处」。一帧而不是 + # _release() 的整段:那一段要 0.2 s,急停的每一毫秒都要。 + self._emit(pos, 0.0, 0.0, kp=0.0, kd=0.0) + log.info("急停松开:已撤力(撤力时实测夹持力 %.1f N),不发张开运动", + st.force_n) + return ReleaseResult( + ok=False, + state=g.get_state(wait=False), + opened_mm=st.position_mm, + target_rad=pos, + aborted=True, + ) + + s = gcfg.close_sign + current_mm = (gcfg.pos_closed_rad - pos) * s * gcfg.rad_to_mm + with self._release_lock: + grip_rad = self._grip_rad + base_mm = current_mm + if grip_rad is not None: + grip_mm = (gcfg.pos_closed_rad - grip_rad) * s * gcfg.rad_to_mm + base_mm = max(current_mm, grip_mm) + if grip_mm < current_mm: + log.info("松开:夹爪已被推外 %.1f mm(夹取点 %.1f mm,现在 " + "%.1f mm),从更外的位置起算", current_mm - grip_mm, + grip_mm, current_mm) + target_mm = base_mm + req.clearance_mm + target_rad = gcfg.pos_closed_rad - s * target_mm / gcfg.rad_to_mm + target_rad = clamp_to_travel(gcfg, target_rad) + # 报告用的是夹过之后的目标开口量:被标定张开限位夹住时,实际能张到 + # 的只有那么多。 + opened_mm = (gcfg.pos_closed_rad - target_rad) * s * gcfg.rad_to_mm + + log.info("松开:从 %.1f mm 再张开 %.1f mm → 目标 %.1f mm(%.5f rad)", + base_mm, req.clearance_mm, opened_mm, target_rad) + move = self._move_to_limit("open", cfg.grasp_speed_mm_s, + target_rad=target_rad) + return ReleaseResult( + ok=move.ok, + state=move.state, + opened_mm=opened_mm, + target_rad=move.target_rad, + aborted=False, + ) + + def _release_timeout_result(self, req: _ReleaseRequest) -> ReleaseResult: + """等夹取线程超时后的结果:报 ``ok=False``,状态尽力读一次。 + + ``wait=False`` 的读数拿的是缓存,不为了这条失败路径去等一帧新状态。 + """ + try: + st = self._g.get_state(wait=False) + except LiteGripError: # 总线已经报错时也要有结果 + st = GripperState() + return ReleaseResult( + ok=False, + state=st, + opened_mm=st.position_mm, + target_rad=st.position_rad, + aborted=req.abort, + ) + def _emit(self, q: float, dq: float = 0.0, tau: float = 0.0, kp: Optional[float] = None, kd: Optional[float] = None) -> float: """下发一帧 MIT 并等待一帧的时间。 @@ -1185,6 +1559,7 @@ def _move_to_limit( hist = [before.position_rad] stalled = False protected = False + release_requested = False # 松开请求叫停了带力接近段 over_torque = 0 # 连续过阈的采样点数 prev_sample_pos = before.position_rad st = before @@ -1194,6 +1569,17 @@ def _move_to_limit( clamped_once = False # 本次移动只记一条封顶 debug 日志 for i in range(1, ceiling + 1): + # 松开请求在**每一帧之前**查,不是按采样或按片查:5 ms 一帧,请求 + # 落下来最多一帧就被看到。只对带设定力的接近段生效 —— 这一段的 + # 终点是用户要的夹取,松开请求的意思就是「别夹了」,继续走完再撤 + # 是另一回事;而 open/close/ramp_to 有自己的终点,不该被松开请求 + # 从半路叫停。 + if force_n is not None and self._release_pending(): + release_requested = True + log.info("带力接近段中止:收到松开请求,上一条指令停在 %+.5f rad" + "(目标 %+.5f);改由 grasp() 执行松开。", + last_cmd, target) + break st = g.get_state(wait=False) pos = st.position_rad # 排程相位。不带力时按计时切 —— ``ramp_steps`` 就是排程跑完的那一 @@ -1316,6 +1702,7 @@ def _move_to_limit( final_cmd_rad=last_cmd, steps=last_i, protected=protected, + release_requested=release_requested, ) def _hold_force( @@ -1358,7 +1745,10 @@ def _hold_force( 每个回读片。配置没标定时不启用 —— 占位限位描述的不是真实行程。经 ``grasp()`` 进来时标定一定在:它的闭合段先过 ``_move_to_limit``。 - ``hold_s <= 0`` 表示不限时长(直到出错、被护栏中止或 Ctrl+C)。 + ``hold_s <= 0`` 表示不限时长(直到出错、被护栏中止、收到松开请求或 + Ctrl+C)。松开请求在**每一帧之前**(5 ms)查一次,不按 + :attr:`MotionConfig.hold_interval` 的 0.2 s 片查 —— 请求一到就停保力, + 结果里 ``release_requested=True``,张开/急停由 ``grasp()`` 接手。 Returns: :class:`HoldResult`。 @@ -1403,6 +1793,16 @@ def _hold_force( # 用上一次读到的位置当 q 下发(kp=0 时 q 不产生任何力,只是给下游 # 一个不越位的指令),每帧把力矩朝设定值推一步,再回读状态查故障 for _ in range(frames_per_slice): + # 松开请求在**每一帧之前**查,不是一片查一次:一片 0.2 s, + # 按片查就意味着松开按钮最多要等 200 ms 才生效;按帧查是 + # 一帧(5 ms)。请求不在这里消费,留在 _release_req 里由 + # grasp() 取走 —— 这一层只负责「停下保力」,张开/急停的语义 + # 属于 grasp()。 + if self._release_pending(): + log.info("保力中止:收到松开请求(已保 %d 片),交给 grasp() " + "执行松开", cycles) + return HoldResult(ok=False, cycles=cycles, state=st, + release_requested=True) tau_cmd = _toward(tau_cmd, target_nm, step_nm) self._emit(pos, 0.0, tau_cmd, kp=0.0, kd=0.0) cycles += 1 diff --git a/src/litegrip/gripper.py b/src/litegrip/gripper.py index 8085dfd..2c76caf 100644 --- a/src/litegrip/gripper.py +++ b/src/litegrip/gripper.py @@ -42,6 +42,7 @@ MotionConfig, MoveProgress, MoveResult, + ReleaseResult, WriteZeroResult, _check_calibrated, clamp_to_travel, @@ -1381,18 +1382,93 @@ def grasp( Args: force_n: Gripping force in N; ``None`` = ``MotionConfig.force_n``. - hold_s: Hold time in seconds; ``0`` = hold until a fault or - Ctrl+C. + hold_s: Hold time in seconds; ``0`` = hold until a fault, + Ctrl+C, or a :meth:`release` / :meth:`request_release` from + another thread. progress: Optional progress callback. Returns: :class:`~litegrip.actions.GraspResult` — truthy when the hold ended normally. ``stalled=True`` with ``reached=False`` means 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. """ self._check_connected() return self._actions.grasp(force_n, hold_s, progress=progress) + def release( + self, + clearance_mm: float = 10.0, + *, + abort: bool = False, + timeout_s: Optional[float] = None, + ) -> ReleaseResult: + """End an in-progress :meth:`grasp` and open the jaws — thread-safe. + + Call it from any thread, including while ``grasp(hold_s=0)`` is blocked + in another one. The grasp thread owns the CAN frames, so this method + never sends any itself while a grasp is running: it records the request + and waits for the grasp thread to finish releasing, then returns that + outcome. + + With no grasp running it performs the release in the calling thread. + + The jaws open from the pose where the grip was **declared** (the end of + the force-carrying approach), not from where the set force pressed them + to: a 20 N grip drives a few millimetres further in, and opening from + there would free that much less. The target is + ``max(current, grip) + clearance_mm``, clamped to the calibrated open + limit, reached by the same bounded position ramp as every other move — + never a step command. + + ``abort=True`` is the E-stop form: it drops the torque and sends no + opening move, returning as soon as the grasp thread has done so. + + Calling it twice is safe: a second call during the same grasp joins the + first and returns its outcome instead of opening again. + + Args: + clearance_mm: Extra opening beyond the grip pose, in mm; must be a + non-negative finite number. + abort: E-stop: torque off only, no opening move. + timeout_s: How long to wait for the grasp thread; ``None`` = + :attr:`MotionConfig.release_wait_s` (10 s). On timeout the + result is ``ok=False`` and a warning is logged. + + Returns: + :class:`~litegrip.actions.ReleaseResult`. + + Raises: + CommandError: ``clearance_mm`` is not a non-negative finite number, + or no grasp is running and the config is uncalibrated. + NotInitializedError: no grasp is running and the gripper is not + connected/enabled. + """ + self._check_connected() + return self._actions.release(clearance_mm, abort=abort, + timeout_s=timeout_s) + + def request_release(self, clearance_mm: float = 10.0, *, + abort: bool = False) -> bool: + """Non-blocking form of :meth:`release`; never touches the CAN bus. + + Records the request if a grasp is running and returns ``True``; returns + ``False`` when there is no grasp to release. Safe for a UI thread, a + signal handler or an E-stop callback: it takes only the release lock, + never ``_io_lock``, and sends no frame. The grasp thread performs the + release before its next frame (within 5 ms). + + Args: + clearance_mm: Extra opening beyond the grip pose, in mm. + abort: E-stop: torque off only, no opening move. + + Returns: + True if a grasp was active and the request was recorded. + """ + return self._actions.request_release(clearance_mm, abort=abort) + def zero(self) -> CalibrationData: """Full calibration: probe both mechanical limits and save. diff --git a/tests/test_actions.py b/tests/test_actions.py index 96f6bac..c05fa36 100644 --- a/tests/test_actions.py +++ b/tests/test_actions.py @@ -2,13 +2,16 @@ from __future__ import annotations +import threading +import time import unittest import _sdkpath # noqa: F401 from litegrip import (CommandError, GraspResult, GripperConfig, GripperState, - LiteGrip, MotionConfig, UnitConversion, clamp_to_travel, - closing_travel_excursion_rad, force_approach_terms, - limit_target, press_target, work_limit_target) + LiteGrip, MotionConfig, ReleaseResult, UnitConversion, + clamp_to_travel, closing_travel_excursion_rad, + force_approach_terms, limit_target, press_target, + work_limit_target) from litegrip.actions import GripperActions, MoveResult from fake_can import (DT, POS_CLOSED_RAD, POS_OPEN_RAD, RAD_TO_MM, @@ -986,6 +989,233 @@ def test_neither_reached_nor_stalled_is_not_handed_over(self): self.assertTrue(any(f.kp == 0.0 for f in fake.frames), "没有撤力帧") +class TestRelease(unittest.TestCase): + """松开:跨线程结束 ``grasp()``、从交接点张余量、急停、不重复张开。 + + 线程用例里 ``sleep_fn`` 用 ``time.sleep(0)`` 而不是空函数:空函数下夹取 + 线程会独占 GIL 一直跑,主线程的请求要等解释器切片的 5 ms 才递得进去 —— + 用例还是能过,但等待时间不确定。``sleep(0)`` 只是让出 GIL,不真的睡。 + """ + + OBJECT_RAD = (POS_OPEN_RAD + POS_CLOSED_RAD) / 2 + + def _motion(self, **kw) -> MotionConfig: + return MotionConfig(sleep_fn=lambda _: time.sleep(0), + monotonic_fn=tick_clock(0.1), **kw) + + def _start_hold(self, g, progress=None): + """后台线程里起一个 ``grasp(hold_s=0)``。 + + Returns: + ``(线程, 结果容器, 保力已开始的事件)`` + """ + started = threading.Event() + outcome = {} + + def report(p): + if p.phase == "hold": + started.set() + if progress is not None: + progress(p) + + def run(): + outcome["res"] = g.grasp(force_n=20.0, hold_s=0.0, progress=report) + + t = threading.Thread(target=run, daemon=True) + t.start() + return t, outcome, started + + def test_release_from_another_thread_ends_a_zero_hold_and_opens_from_the_grip_pose(self): + """``hold_s=0`` 在另一个线程里,``release()`` 把它结束并从交接点张 10 mm。 + + 工件会屈服(``yield_rad_s``):设定力在保力期间把夹爪往闭合侧继续推, + 所以松开那一刻的位置**不是**交接点。基准必须取交接点 —— 也就是从夹爪 + 声明抓住的位置再张 10 mm,而不是从被压实后的位置。 + """ + g, fake = make_gripper(block_rad=self.OBJECT_RAD, + yield_rad_s=0.05, yield_tau_nm=0.5, + motion=self._motion()) + t, outcome, started = self._start_hold(g) + self.assertTrue(started.wait(2.0), "保力段没有开始") + grip_rad = g.actions._grip_rad + self.assertIsNotNone(grip_rad, "交接点没有记录") + # 等保力把夹爪往里再压一段(工件屈服),这样「交接点」与「停下处」 + # 才是两个不同的数;否则这条用例分不出用哪个当基准。 + deadline = time.monotonic() + 2.0 + while fake.motor.pos - grip_rad < 0.01 and time.monotonic() < deadline: + time.sleep(0.001) + self.assertGreater(fake.motor.pos - grip_rad, 0.01, + "工件没有屈服,用例分不出基准") + hold_from = len(fake.frames) + + result = g.release(clearance_mm=10.0, timeout_s=5.0) + t.join(2.0) + self.assertFalse(t.is_alive(), "grasp 没有在松开后返回") + + res = outcome["res"] + self.assertTrue(res.released, res) + self.assertTrue(res.ok, res) + self.assertIsInstance(result, ReleaseResult) + self.assertTrue(result.ok, result) + self.assertFalse(result.aborted) + + # 基准按**交接点**复算;若错用了松开那一刻的位置,目标会少那几毫米。 + gcfg = g.config + s = gcfg.close_sign + grip_mm = (gcfg.pos_closed_rad - grip_rad) * s * gcfg.rad_to_mm + expected_mm = grip_mm + 10.0 + expected_rad = gcfg.pos_closed_rad - s * expected_mm / gcfg.rad_to_mm + self.assertAlmostEqual(result.target_rad, expected_rad, places=6) + self.assertAlmostEqual(result.opened_mm, expected_mm, places=4) + self.assertAlmostEqual(res.state.position_rad, expected_rad, + delta=g.motion_config.reach_tol) + + # 张开是斜坡:保力之后有一串位置帧,第一帧不是一步跳到目标。 + opens = [f for f in fake.frames[hold_from:] if f.kp != 0.0] + self.assertGreater(len(opens), 5, "张开不是一段斜坡而是阶跃") + first_pos = opens[0].pos_after + self.assertLess(abs(opens[0].q - first_pos), + abs(expected_rad - first_pos) / 2, + "第一帧就跳到了目标附近") + # 正装时张开是 rad 减小,整段单调。 + self.assertTrue(all(b.q <= a.q + 1e-9 + for a, b in zip(opens, opens[1:]))) + + def test_release_during_the_approach_aborts_it(self): + """接近途中按下松开:不再交接,改从当前位置张开。""" + g, fake = make_gripper(motion=self._motion()) + asked = [] + + def progress(p): + if p.phase == "move" and not asked: + asked.append(True) + self.assertTrue(g.request_release(clearance_mm=5.0)) + + res = g.grasp(force_n=20.0, hold_s=0.0, progress=progress) + + self.assertTrue(asked, "接近段没有进度回调,用例没测到") + self.assertTrue(res.released, res) + self.assertTrue(res.ok, res) + self.assertEqual(res.cycles, 0, "接近段被中止了却还交接了保力") + # 一帧保力帧都不该发(保力帧是唯一带 tau_ff 的帧)。 + self.assertFalse([f for f in fake.frames if f.tau_ff != 0.0]) + # 还没交接 → 基准是当前位置;当前位置离张开端不足 5 mm,目标被夹到 + # 标定张开限位。 + self.assertAlmostEqual(res.state.position_rad, POS_OPEN_RAD, + delta=g.motion_config.reach_tol) + + def test_abort_sends_no_open_ramp_and_returns_promptly(self): + """``abort=True``:只撤力、不发张开运动,尽快返回。""" + g, fake = make_gripper(block_rad=self.OBJECT_RAD, motion=self._motion()) + t, outcome, started = self._start_hold(g) + self.assertTrue(started.wait(2.0), "保力段没有开始") + abort_from = len(fake.frames) + + t0 = time.monotonic() + result = g.release(abort=True, timeout_s=5.0) + elapsed = time.monotonic() - t0 + t.join(2.0) + self.assertFalse(t.is_alive(), "急停没有让 grasp 返回") + + res = outcome["res"] + self.assertTrue(result.aborted) + self.assertFalse(result.ok) + self.assertFalse(res.ok, res) + self.assertTrue(res.released, res) + self.assertLess(elapsed, 1.0, "急停等了 %.2f s" % elapsed) + # 急停之后只有零增益帧:没有张开斜坡。 + tail = fake.frames[abort_from:] + self.assertTrue(tail, "急停一帧都没发") + self.assertTrue(all(f.kp == 0.0 for f in tail), + "急停发了带刚度的张开帧") + + def test_release_without_a_grasp_opens_from_the_current_position(self): + """没有夹取在跑:在调用方线程里直接张,基准是当前位置。""" + g, fake = make_gripper(start_rad=POS_CLOSED_RAD, stops=True, + motion=self._motion()) + + result = g.release(clearance_mm=5.0) + + gcfg = g.config + expected_rad = (gcfg.pos_closed_rad + - gcfg.close_sign * 5.0 / gcfg.rad_to_mm) + self.assertTrue(result.ok, result) + self.assertFalse(result.aborted) + self.assertAlmostEqual(result.target_rad, expected_rad, places=6) + self.assertAlmostEqual(result.opened_mm, 5.0, places=4) + self.assertTrue(fake.frames, "一帧都没发") + self.assertAlmostEqual(result.state.position_rad, expected_rad, + delta=g.motion_config.reach_tol) + + def test_a_second_release_does_not_open_twice(self): + """同一个夹取里第二次 ``release()`` 直接拿第一次的结果,不再张开。""" + g, _ = make_gripper(block_rad=self.OBJECT_RAD, motion=self._motion()) + opening = threading.Event() + + t, outcome, started = self._start_hold(g) + self.assertTrue(started.wait(2.0), "保力段没有开始") + + calls = [] + original = g.actions._do_release + + def counted(req): + calls.append(req) + opening.set() + # 等第二次请求真的排进来再放行张开斜坡 —— 这样「第二次调用发生在 + # 第一次的松开进行中」是确定的,不靠线程调度。 + deadline = time.monotonic() + 2.0 + while not g.actions._release_pending() and time.monotonic() < deadline: + time.sleep(0.001) + return original(req) + + g.actions._do_release = counted + + results = [] + + def first_release(): + results.append(g.release(clearance_mm=10.0, timeout_s=5.0)) + + first = threading.Thread(target=first_release, daemon=True) + first.start() + self.assertTrue(opening.wait(2.0), "第一次松开没有开始张开") + second = g.release(clearance_mm=10.0, timeout_s=5.0) + + first.join(2.0) + t.join(2.0) + self.assertFalse(first.is_alive()) + self.assertFalse(t.is_alive(), "grasp 没有返回") + self.assertEqual(len(calls), 1, "第二次 release 又张开了一次") + self.assertTrue(all(r.ok for r in results + [second])) + self.assertTrue(outcome["res"].released) + + def test_request_release_alone_does_not_send_frames(self): + """``request_release`` 只递请求:不碰 CAN,也不获取 ``_io_lock``。""" + g, fake = make_gripper(motion=self._motion()) + # 没有夹取:返回 False,且总线上没有任何帧。 + self.assertFalse(g.request_release(clearance_mm=3.0)) + self.assertEqual(fake.frames, []) + + # 有夹取:把夹取线程停在保力的进度回调里,再确认这个调用本身不发帧。 + parked = threading.Event() + let_go = threading.Event() + + def progress(p): + if p.phase == "hold" and not parked.is_set(): + parked.set() + let_go.wait(2.0) + + t, outcome, started = self._start_hold(g, progress=progress) + self.assertTrue(parked.wait(2.0), "夹取线程没有停在保力回调里") + before = len(fake.frames) + self.assertTrue(g.request_release(clearance_mm=3.0)) + self.assertEqual(len(fake.frames), before, + "request_release 自己发了帧") + let_go.set() + t.join(2.0) + self.assertFalse(t.is_alive()) + self.assertTrue(outcome["res"].released) + + class TestForceHoldIsTorqueOnly(unittest.TestCase): """保力只下发前馈力矩(kp=kd=0):力不随工件屈服而衰减。 From 4e279b658161ec3bf2a4352bd2bfcd4e97dc55c9 Mon Sep 17 00:00:00 2001 From: cao-xiao-hao <127832643+cao-xiao-hao@users.noreply.github.com> Date: Thu, 29 Oct 2026 16:43:59 +0800 Subject: [PATCH 2/3] docs: document the release api --- README.md | 28 ++++++++++++++++++++++++---- 1 file changed, 24 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 2319e47..a32ff11 100644 --- a/README.md +++ b/README.md @@ -174,7 +174,9 @@ 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, or Ctrl+C. `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). `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. | | `goto_rad(position_rad, kp=None, kd=None, dq_target=0.0, tau_feedforward=0.0, duration=0.5)` → `MoveResult` | The same in radians. `dq_target` and `tau_feedforward` must be 0: the ramp derives the velocity feed-forward from its own schedule, so both are refused with `CommandError` rather than ignored. | | `move_to(target_rad, kp=None, kd=None, tau_feedforward=0.0, duration=1.0)` → `MoveResult` | The same as `goto_rad`, with a longer default `duration`. | @@ -204,6 +206,18 @@ and the hold frames carry `kp = kd = 0`, so the drive's output *is* the feed-for `force_n × 0.1 Nm`. That is what makes the force independent of the workpiece: as a soft object yields, the jaws follow it and the grip stays at the setpoint. +**The hold is ended by `release()`, and only by it.** A `grasp(hold_s=0)` keeps sending hold frames +until something stops it; there is no separate "stop pressing, stay where you are" call. `release()` +opens from the pose the grip was *declared* at — the end of the force-carrying approach — by +`clearance_mm` more, not from wherever the set force pressed the jaws to: a 20 N grip drives a few +millimetres further in, and opening from there would free that much less. The target is +`max(current, grip pose) + clearance_mm`, clamped to the calibrated open stop and reached by the +same bounded ramp as every other move. Because a grasp may be holding in one thread while the caller +wants it to stop from another, `release()` never sends frames from the caller's thread: it records +the request, the grasp thread sees it before its next frame (5 ms) and performs the opening, and the +caller gets that result. `request_release()` is the same without the wait, for threads that must not +block. `abort=True` drops the torque and sends no opening move — the E-stop form. + **A force is only meaningful against something.** There is no call that applies a force at the current position, and that is deliberate. Such a call cannot tell whether anything is between the jaws, and the hold frames give the drive no position or velocity error to work against — with @@ -346,7 +360,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` | `ok` | +| `GraspResult` | `ok`, `reached`, `stalled`, `state`, `target_rad`, `force_n`, `cycles`, `travel_guard`, `released` | `ok` | +| `ReleaseResult` | `ok`, `state`, `opened_mm`, `target_rad`, `aborted` | `ok` | | `EnableResult` | `ok`, `state`, `tries` | `ok` | **Read `ok`.** Three combinations look alarming and are correct: @@ -371,6 +386,10 @@ 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()` 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. + Stalling far from the limit gives `ok=False`: something blocked the travel. The diagnostic fields — `final_cmd_rad`, `steps`, `state` — are what you log when it is false. @@ -422,6 +441,7 @@ with LiteGrip("can0") as gripper: | `force_n` | `20.0` | default `grasp` force | | `force_ramp_n_s` | `20.0` | rate the held force climbs to its setpoint, N/s | | `hold_interval` | `0.2` | force-hold slice length, seconds | +| `release_wait_s` | `10.0` | how long `release()` waits for the grasp thread when `timeout_s` is not given | | `hold_runaway_rad` | `0.05` | past the closed stop by this much, the force hold drops the torque and stops | | `margin` | `0.05` | fraction of travel kept in reserve from the stop by the `limit_target` helper; no shipped call reaches it | | `max_lead_mm` | `4.0` | cap on how far the command may lead the measured position, on moves that carry no force setpoint | @@ -553,8 +573,8 @@ python3 examples/trajectory.py --play pick --repeat 3 - **A capture that did not fill raises** rather than returning a short recording as if it were whole. - **Record without zero-gravity when something else drives.** `record_start(zero_gravity=False)` - only *reads* state, so the caller may run a `grasp()` or a move sequence from another thread and - capture it. + only *reads* state, so the caller may run a `grasp()` (ended with `release()`) or a move sequence + from another thread and capture it. - `LITEGRIP_TRAJ_DIR` moves the trajectory directory. Recording, replay and teleop are mutually exclusive and raise `TrajectoryBusyError` / `TeleopBusyError`; `disconnect()` stops whichever is running. From 78b8766c920cb0b7f0b35584c70b8ff33186272a Mon Sep 17 00:00:00 2001 From: cao-xiao-hao <127832643+cao-xiao-hao@users.noreply.github.com> Date: Thu, 29 Oct 2026 17:18:39 +0800 Subject: [PATCH 3/3] chore: remove internal bench check scripts These are hardware bench tools for this specific setup, not part of the published SDK: both hardcode absolute /home paths, and one is a one-off torque guard sweep. The pytest suite does not collect them, so nothing else changes. --- tests/torque_guard_check.py | 395 --------------------------- tests/trajectory_check.py | 522 ------------------------------------ 2 files changed, 917 deletions(-) delete mode 100644 tests/torque_guard_check.py delete mode 100644 tests/trajectory_check.py diff --git a/tests/torque_guard_check.py b/tests/torque_guard_check.py deleted file mode 100644 index 49d1901..0000000 --- a/tests/torque_guard_check.py +++ /dev/null @@ -1,395 +0,0 @@ -"""Runs the teleop follower's torque guard end to end and prints what it checked. - -For anyone who wants to see the guard work — and to see where it does not — -before trusting it: run - - cd /home/qaz/litegrip-python - python3 tests/torque_guard_check.py - -It drives a follower into a rigid obstacle with a leader the check steers itself, -and prints every number behind each claim the docs make: that the guard trips on -the press, goes to zero stiffness and damping in place, keeps streaming so the -motor does not latch its comm-loss fault, does not chatter while the leader holds -at the stop, re-arms once the leader has reopened past `TORQUE_REARM_OPENNESS`, -and trips again on the next stroke. It then runs the bench command the README and -`examples/teleop.py` tell a user to run, and checks that it does what they say. - -Each step prints ``ok`` or ``FAIL``, and the exit code is 1 if any step failed, so -it can also be run from a script. - -This is not the test suite. That is ``tests/test_example_teleop.py``, which covers -the guard case by case under ``unittest``; this file is deliberately not named -``test_*`` so ``unittest discover`` does not collect it twice. What it adds is one -run whose output a person can read. - -It proves how the guard behaves against the kinematic fake CAN in -``tests/fake_can.py``. It does **not** prove anything about real hardware: the -fake moves its jaw at ``v = dq + GAIN * (q - pos)`` and reports ``tau = kp * (q - -pos)``, so its torque is a function of following error alone. A real motor reports -current, and its `kd * dq` term is a torque term the fake does not model. - -Worse, the fake integrates a fixed ``DT = 0.005`` per frame whatever the loop rate, -so it only keeps time at 200 Hz. At the example's default 50 Hz it runs four times -slower than the loop, the follower never catches up, and a free sweep with nothing -between the jaws reads about 2 Nm. The documented bench command passes -``--torque-limit 1.0`` at that rate, so on the fake it trips before it reaches -anything — a property of the fake, which the run prints as a note rather than -scoring it against the guard. Numbers from this run are evidence about the guard's -logic, not about a gripper. -""" - -from __future__ import annotations - -import importlib.util -import math -import os -import sys -import threading -import time -from pathlib import Path - - -def _find_root() -> Path: - """The repo root, so this file runs from ``tests/`` and from anywhere else.""" - candidates = [] - if "LITEGRIP_REPO" in os.environ: - candidates.append(Path(os.environ["LITEGRIP_REPO"])) - here = Path(__file__).resolve() - candidates += [Path.cwd(), here.parent, here.parent.parent] - for candidate in candidates: - if ((candidate / "src" / "litegrip").is_dir() - and (candidate / "tests" / "fake_can.py").is_file()): - return candidate.resolve() - raise SystemExit("run this from the litegrip-python repo root, " - "or set LITEGRIP_REPO to it") - - -_ROOT = _find_root() -sys.path[:0] = [str(_ROOT / "src"), str(_ROOT / "tests")] - -from litegrip import InProcTeleopTransport, encode_frame, teleop_topic # noqa: E402 -from litegrip.teleop import rad_to_openness # noqa: E402 -from fake_can import (POS_CLOSED_RAD, POS_OPEN_RAD, # noqa: E402 - RAD_TO_MM, make_gripper) - -_EXAMPLE = _ROOT / "examples" / "teleop.py" -_spec = importlib.util.spec_from_file_location("example_teleop", _EXAMPLE) -example = importlib.util.module_from_spec(_spec) -_spec.loader.exec_module(example) - -# ── the bench this check runs on ──────────────────────────────────────────── -# The obstacle sits half way through the stroke: the follower presses on it well -# clear of either end, so a trip here cannot be confused with one at full open. -OBSTACLE_RAD = (POS_OPEN_RAD + POS_CLOSED_RAD) / 2.0 -TRAVEL_MM = abs(POS_OPEN_RAD - POS_CLOSED_RAD) * RAD_TO_MM -TOPIC = teleop_topic("gripA") - -# The configuration the guard is documented to work in: their own test's rates, -# where the sweep is slow enough that following alone never crosses the limit. -RATE_HZ = 200.0 -RAMP_PER_S = 0.4 -LIMIT_NM = 1.0 - -# The bench command the README and the example print for a user to run. -DOCUMENTED_ARGV = ["--mode", "slave", "--fake-leader", "--torque-limit", "1.0"] - -_results: list[bool] = [] - - -def check(label: str, ok: bool, detail: str = "") -> bool: - _results.append(bool(ok)) - print(f"{'ok ' if ok else 'FAIL'} {label}" + (f"\n {detail}" if detail else "")) - return bool(ok) - - -def note(label: str, detail: str = "") -> None: - """Something worth reading that is not a claim about the guard.""" - print(f"note {label}" + (f"\n {detail}" if detail else "")) - - -def wait(predicate, timeout_s: float) -> bool: - deadline = time.monotonic() + timeout_s - while time.monotonic() < deadline: - if predicate(): - return True - time.sleep(0.002) - return bool(predicate()) - - -class Leader: - """A leader the check steers, so the sweep is exact rather than timed. - - ``examples.teleop._FakeLeader`` sweeps a triangle and cannot be told to hold - still, which the no-chatter check needs. This publishes whatever opening it - is handed, at the follower's rate. - """ - - def __init__(self, bus, topic: str, rate_hz: float) -> None: - self._bus, self._topic = bus, topic - self._dt = 1.0 / rate_hz - self.openness = 1.0 - self._halt = threading.Event() - self._thread: threading.Thread | None = None - - def start(self) -> None: - self._thread = threading.Thread(target=self._run, name="check-leader", - daemon=True) - self._thread.start() - - def stop(self) -> None: - self._halt.set() - if self._thread is not None: - self._thread.join(timeout=1.0) - - def ramp(self, target: float, rate: float = RAMP_PER_S, - timeout_s: float = 15.0) -> bool: - """Walk the published opening to ``target``; True if it got there.""" - deadline = time.monotonic() + timeout_s - while time.monotonic() < deadline: - delta = target - self.openness - if abs(delta) <= rate * self._dt: - self.openness = target - return True - self.openness += math.copysign(rate * self._dt, delta) - time.sleep(self._dt) - return False - - def _run(self) -> None: - while not self._halt.is_set(): - t0 = time.monotonic() - self._bus.pub(self._topic, encode_frame( - self.openness, self.openness * TRAVEL_MM, 0.0, time.time())) - time.sleep(max(0.0, self._dt - (time.monotonic() - t0))) - - -def follower(limit_nm: float, rate_hz: float = RATE_HZ): - """A calibrated, enabled follower in front of the obstacle.""" - g, fake = make_gripper(start_rad=POS_OPEN_RAD, block_rad=OBSTACLE_RAD) - g.motion_config.sleep_fn = time.sleep # real pacing: watch it in time - bus = InProcTeleopTransport() - g.teleop_start("slave", transport=bus, align=False, - torque_limit_nm=limit_nm, rate_hz=rate_hz) - return g, fake, bus - - -def free_sweep_peak(rate_hz: float, openness_rate: float = 0.3) -> float: - """Peak torque the fake reports following a sweep with nothing to press. - - No obstacle is installed, so every newton-metre here is the fake's own - tracking error. It is the yardstick for reading a trip that happens before - the follower has reached anything. - """ - g, _ = make_gripper(start_rad=POS_OPEN_RAD) # no block_rad - g.motion_config.sleep_fn = time.sleep - bus = InProcTeleopTransport() - g.teleop_start("slave", transport=bus, align=False, torque_limit_nm=0.0, - rate_hz=rate_hz) - leader = Leader(bus, TOPIC, rate_hz) - leader.start() - try: - threading.Thread(target=leader.ramp, - args=(1.0 - openness_rate * 1.5, openness_rate), - daemon=True).start() - peak = 0.0 - deadline = time.monotonic() + 1.5 - while time.monotonic() < deadline: - peak = max(peak, abs(g.teleop_status()["torque_nm"])) - time.sleep(0.005) - return peak - finally: - leader.stop() - g.teleop_stop() - - -# ── the guard, in the configuration it is documented to work in ───────────── - -def check_guard_engages() -> None: - g, fake, bus = follower(LIMIT_NM) - print(f"\nthe guard, at {RATE_HZ:.0f} Hz against an obstacle at openness " - f"{rad_to_openness(OBSTACLE_RAD, g.config):.2f}") - leader = Leader(bus, TOPIC, RATE_HZ) - leader.start() - try: - sweep = threading.Thread(target=leader.ramp, args=(0.0,), daemon=True) - sweep.start() - - tripped = wait(lambda: g.teleop_status()["over_torque"], 15.0) - st = g.teleop_status() - check("trips when the follower presses on the obstacle", - tripped and st["openness"] <= 0.9, - f"over_torque={st['over_torque']} at leader openness " - f"{st['openness']:.3f}, torque {st['torque_nm']:.2f} Nm vs limit " - f"{LIMIT_NM:.2f} Nm\n the trip must come from the press, not " - f"from the transient at start-up: openness stays under 0.9") - - # Released in place: zero stiffness and zero damping, but the frames keep - # flowing so the motor does not latch its comm-loss fault. - mark = len(fake.frames) - time.sleep(0.2) - sent = fake.frames[mark:] - gains = sorted({(f.kp, f.kd) for f in sent}) - check("every frame sent while tripped is zero-stiffness, zero-damping", - bool(sent) and all(f.kp == 0.0 and f.kd == 0.0 for f in sent), - f"{len(sent)} frames, gains {gains}") - check("it keeps streaming while tripped", - len(sent) >= RATE_HZ * 0.15, - f"{len(sent)} frames in 200 ms at {RATE_HZ:.0f} Hz; a silent " - f"follower would latch the motor's comm-loss fault") - - # Latching: held at the stop, it stays released instead of chattering - # back on against the same obstruction. - sweep.join(timeout=10.0) - trips_before = g.teleop_status()["torque_trips"] - mark = len(fake.frames) - time.sleep(0.5) - sent = fake.frames[mark:] - reengaged = sum(1 for f in sent if f.kp > 0.0) - check("it stays released while the leader holds at the stop", - g.teleop_status()["over_torque"] - and g.teleop_status()["torque_trips"] == trips_before - and reengaged == 0, - f"trips {trips_before} -> {g.teleop_status()['torque_trips']}, " - f"{reengaged} re-engaging frames of {len(sent)} in 500 ms") - - # Re-arm: the leader reopens past the margin. - trip_openness = st["openness"] - target = min(1.0, trip_openness + 0.15) - threading.Thread(target=leader.ramp, args=(target,), daemon=True).start() - rearmed = wait(lambda: not g.teleop_status()["over_torque"], 15.0) - check(f"re-arms once the leader reopens past " - f"{0.05:.2f} (tripped at openness {trip_openness:.3f}, " - f"reopened to {target:.3f})", - rearmed, f"over_torque={g.teleop_status()['over_torque']} at " - f"openness {g.teleop_status()['openness']:.3f}") - - mark = len(fake.frames) - time.sleep(0.1) - sent = fake.frames[mark:] - check("it follows again after re-arming", - bool(sent) and all(f.kp > 0.0 for f in sent), - f"gains {sorted({(f.kp, f.kd) for f in sent})}") - - # And trips again on the next stroke, rather than latching off. - trips_before = g.teleop_status()["torque_trips"] - threading.Thread(target=leader.ramp, args=(0.0,), daemon=True).start() - again = wait(lambda: g.teleop_status()["torque_trips"] > trips_before, 15.0) - check("trips again on the next stroke", - again, f"trips {trips_before} -> {g.teleop_status()['torque_trips']}") - finally: - leader.stop() - g.teleop_stop() - - -def check_guard_off_by_default() -> None: - print("\nthe guard off (``torque_limit_nm=0``, the SDK default)") - g, fake, bus = follower(0.0) - leader = Leader(bus, TOPIC, RATE_HZ) - leader.start() - try: - sweep = threading.Thread(target=leader.ramp, args=(0.0,), daemon=True) - sweep.start() - sweep.join(timeout=10.0) - time.sleep(0.5) - st = g.teleop_status() - sent = fake.frames[-int(RATE_HZ * 0.2):] - check("leaves the follower pressing, with no trip", - st["torque_trips"] == 0 and not st["over_torque"], - f"trips={st['torque_trips']} over_torque={st['over_torque']} " - f"torque {st['torque_nm']:.2f} Nm while pressing") - check("and keeps the follow gains on while it presses", - bool(sent) and all(f.kp > 0.0 for f in sent), - f"gains {sorted({(f.kp, f.kd) for f in sent})}") - finally: - leader.stop() - g.teleop_stop() - - -# ── the command the docs tell a user to run ───────────────────────────────── - -def check_documented_command() -> None: - """Run the bench command from README.md and examples/teleop.py verbatim. - - The script itself is not run: it opens a real CAN channel. Everything it - passes to the SDK is reproduced instead, straight from its own parser, so the - only difference is the fake CAN in place of ``can0``. - """ - args = example.build_parser().parse_args(DOCUMENTED_ARGV) - print(f"\nthe documented bench command: " - f"examples/teleop.py {' '.join(DOCUMENTED_ARGV)}") - print(f" defaults it runs with: rate={args.rate:.0f} " - f"openness_rate={args.openness_rate} dq_max={args.dq_max} " - f"kp={args.kp} kd={args.kd} align={not args.no_align}") - - free_peak = free_sweep_peak(args.rate, args.openness_rate) - - bus = InProcTeleopTransport() - g, fake = make_gripper(start_rad=POS_OPEN_RAD, block_rad=OBSTACLE_RAD) - g.motion_config.sleep_fn = time.sleep - leader = example._FakeLeader( - bus, teleop_topic(args.grip_id), TRAVEL_MM, args.rate, args.openness_rate) - g.teleop_start(args.mode, transport=bus, grip_id=args.grip_id, kp=args.kp, - kd=args.kd, align=not args.no_align, watchdog_s=args.watchdog, - dq_max=args.dq_max, rate_hz=args.rate, - torque_limit_nm=args.torque_limit) - leader.start() # started after teleop_start, as the script does - try: - worker = g._teleop - trip = None - rearm = None - strokes = 0 - last_dir = None - t0 = time.monotonic() - while time.monotonic() - t0 < 20.0: - if trip is None and worker._over_torque: - trip = (time.monotonic() - t0, worker._trip_openness, - worker._torque_nm) - if trip is not None and rearm is None and not worker._over_torque: - rearm = (time.monotonic() - t0, worker._last_openness) - break - direction = math.copysign(1.0, leader._direction) - if last_dir is not None and direction != last_dir: - strokes += 1 - last_dir = direction - time.sleep(0.005) - - check("the guard trips, as the docs say it will", - trip is not None, - "no trip in 20 s" if trip is None else - f"tripped at t={trip[0]:.2f}s, leader openness {trip[1]:.3f}, " - f"torque {trip[2]:.2f} Nm vs limit {args.torque_limit:.2f} Nm") - if trip is None: - return - check("it re-arms, as the docs say it will", - rearm is not None, - f"watched {strokes} turns of the sweep, " - f"{time.monotonic() - t0:.1f}s, and the guard never re-armed" - if rearm is None else - f"re-armed at t={rearm[0]:.2f}s, leader openness {rearm[1]:.3f}") - note("where that trip came from is a property of the fake, not the guard", - f"it tripped at openness {trip[1]:.3f}, near full open, before the " - f"sweep reached the obstacle — and {free_peak:.2f} " - f"Nm is what the fake reports for a sweep with **nothing between " - f"the jaws at all** at this rate. FakeMotor integrates a fixed " - f"DT=0.005 per frame whatever the loop rate, so at {args.rate:.0f} " - f"Hz the fake runs four times slower than the loop and the follower " - f"never catches up. This command's follower is real hardware " - f"(--channel can0), which this check cannot reach.") - finally: - leader.stop() - g.teleop_stop() - - -def main() -> int: - started = time.monotonic() - check_guard_engages() - check_guard_off_by_default() - check_documented_command() - - failed = _results.count(False) - print(f"\n{len(_results)} checks, {failed} failed, " - f"{time.monotonic() - started:.0f}s") - return 1 if failed else 0 - - -if __name__ == "__main__": - raise SystemExit(main()) diff --git a/tests/trajectory_check.py b/tests/trajectory_check.py deleted file mode 100644 index 762b1e6..0000000 --- a/tests/trajectory_check.py +++ /dev/null @@ -1,522 +0,0 @@ -"""Runs the whole trajectory record/replay path end to end and prints what it checked. - -For anyone who wants to see the feature work before trusting it: run - - cd /home/qaz/litegrip-python - python3 tests/trajectory_check.py - -It teaches a path, saves it, loads it back, replays it on the unit that recorded -it and on a reverse-mounted one, and prints every number behind each step. Each -step prints ``ok`` or ``FAIL``, and the exit code is 1 if any step failed, so it -can also be run from a script. - -This is not the test suite. That is ``tests/test_trajectory.py``, which covers -the same ground case by case under ``unittest``; this file is deliberately not -named ``test_*`` so ``unittest discover`` does not collect it twice. What it -adds is one run whose output a person can read. - -It proves the recording is the motion that was applied, that the file is the -recording, and that the replay is the file — sample for sample. It does **not** -prove anything about real hardware: the motion comes from the kinematic fake CAN -in ``tests/fake_can.py`` and the clock is a fake, so no servo, no bus and no -mechanics are involved. -""" - -from __future__ import annotations - -import math -import os -import tempfile -import time - -import _sdkpath # noqa: F401 (puts src/ on sys.path) -from litegrip import (InProcTeleopTransport, TeleopBusyError, Trajectory, - TrajectoryBusyError, TrajectoryEmptyError, - TrajectoryError, TrajectoryFormatError) -from litegrip.teleop import openness_to_rad, rad_to_openness - -from fake_can import POS_CLOSED_RAD, POS_OPEN_RAD, RAD_TO_MM, make_gripper - -# ── what is being taught ──────────────────────────────────────────────────── -# -# The hand sweeps the jaws closed -> fully open -> closed over DURATION seconds. -# RATE_HZ is the capture rate; INTERVAL is MotionConfig's ramp frame interval, -# which is also the replay's frame rate (so a replay runs at 200 Hz). -DURATION = 2.0 -RATE_HZ = 100.0 -INTERVAL = 0.005 -ALIGN_S = 1.0 # the goto_rad duration a replay's align move uses -TOL = 1e-9 - - -def profile(t: float) -> float: - """The opening the hand sweeps, as a function of seconds since it started.""" - return 0.5 - 0.5 * math.cos(2.0 * math.pi * t / DURATION) - - -class FakeClock: - """A monotonic clock that moves only when a loop sleeps. - - One cycle of either loop then advances trajectory time by exactly one cycle, - so a capture and a replay run through in microseconds and land on the same - samples every run. That is what lets the checks below be equalities instead - of tolerances around a scheduler. - - ``real_sleep`` adds a short real sleep per cycle, for the checks that need a - session to still be running when the next line executes. - """ - - def __init__(self, on_tick=None, real_sleep: float = 0.0) -> None: - self.t = 0.0 - self._on_tick = on_tick - self._real_sleep = real_sleep - - def now(self) -> float: - return self.t - - def sleep(self, seconds: float) -> None: - self.t += seconds - if self._on_tick is not None: - self._on_tick(self.t) - if self._real_sleep: - time.sleep(self._real_sleep) - - def install(self, g) -> "FakeClock": - g.motion_config.monotonic_fn = self.now - g.motion_config.sleep_fn = self.sleep - return self - - -class Hand: - """Writes the profile onto the fake motor, in step with the fake clock. - - The recorder samples right after its pacing sleep, so a position written - from inside that sleep is the position sampled at that instant: the capture - is the profile exactly, rather than a hand that raced the sampler. - - The real hand-teaching path is ``zero_gravity=True`` — the zero-torque frames - that make the jaws back-drivable. That is not what moves the motor here, - because the fake's slack motor springs toward zero; the hand is written to - the motor directly, and the zero-torque frames are checked on their own. - """ - - def __init__(self, fake, g) -> None: - self._fake = fake - self._g = g - - def __call__(self, t: float) -> None: - openness = profile(t) - self._fake.motor.pos = openness_to_rad(openness, self._g.config) - # The fake derives torque and velocity only from the MIT frames it is - # sent, and a hand-driven capture sends none. They are written here so - # the recorded diagnostics are not all zeros: otherwise a player that - # fed them forward would look exactly like one that does not, and the - # check meant to catch that would pass either way. - self._fake.motor.tau = 2.0 * openness - self._fake.motor.vel = 2.0 - 4.0 * openness - - def park(self, openness: float) -> None: - """Put the jaws somewhere without recording it.""" - self._fake.motor.pos = openness_to_rad(openness, self._g.config) - - -# ── reporting ─────────────────────────────────────────────────────────────── - - -class Report: - """Prints one line per check and counts what failed.""" - - def __init__(self) -> None: - self.passed = 0 - self.failures = 0 - - def section(self, title: str) -> None: - print(f"\n{title}") - - def check(self, name: str, fn) -> None: - """Run *fn*, which returns a detail string or raises.""" - try: - detail = fn() - except Exception as e: # noqa: BLE001 — the failure is the output here - self.failures += 1 - print(f" FAIL {name}") - print(f" {type(e).__name__}: {e}") - return - self.passed += 1 - print(f" ok {name}" + (f" [{detail}]" if detail else "")) - - -def bar(values, width: int = 60) -> str: - """One ASCII row standing in for a plot of *values*, which are in [0, 1].""" - ramp = " .:-=+*#%@" - stride = max(1, len(values) // width) - return "".join(ramp[min(9, max(0, int(round(v * 9))))] - for v in list(values)[::stride][:width]) - - -def require(condition: bool, message: str) -> None: - if not condition: - raise AssertionError(message) - - -def close(a: float, b: float, tol: float = TOL) -> bool: - return abs(a - b) <= tol - - -def commanded_openness(fake, g): - """The opening each streamed frame asked for, in order, minus the hold. - - The final frame is dropped: it is the hold ``play_stop`` sends at the motor's - *measured* position, which lags the trajectory by however long the servo - took, and is therefore not a point on the trajectory. - """ - frames = fake.frames[:-1] if fake.frames else [] - return [rad_to_openness(f.q, g.config) for f in frames] - - -def replay(g, fake, traj, **kwargs): - """Replay once and return the frames it sent, minus the trailing hold.""" - fake.frames = [] - g.play(traj, **kwargs) - return fake.frames[:-1] - - -# ── checks ────────────────────────────────────────────────────────────────── - - -def check_capture_is_the_motion(traj) -> str: - expected = int(DURATION * RATE_HZ) - require(len(traj) == expected, - f"expected {expected} samples over {DURATION}s at {RATE_HZ:.0f}Hz, " - f"got {len(traj)}") - require(close(traj.samples[0].t, 0.0), - f"first sample is at t={traj.samples[0].t}, not 0.0") - worst = max(abs(s.openness - profile(s.t)) for s in traj.samples) - require(worst < TOL, - f"the capture drifts from the hand by {worst:.3e} openness — the " - f"recorder sampled a different instant than the hand moved on") - return (f"{len(traj)} samples, {traj.duration:.3f}s, worst error against " - f"the hand {worst:.1e}") - - -def check_stamps_advance(traj) -> str: - stamps = [s.t for s in traj.samples] - require(all(b > a for a, b in zip(stamps, stamps[1:])), - "timestamps are not strictly increasing") - gaps = [b - a for a, b in zip(stamps, stamps[1:])] - require(max(gaps) - min(gaps) < TOL, - f"sample gaps vary by {max(gaps) - min(gaps):.3e}s") - return f"{len(stamps)} distinct stamps, every gap {gaps[0]:.4f}s" - - -def check_zero_gravity_frames(fake, g, hand) -> str: - """Teach-mode frames are torque-free; the one sent on stop re-engages them. - - Both rules matter and pull in opposite directions: a teaching frame that - carried gains would hold the jaws against the hand, and a recording that - ended without one would leave them slack. - """ - fake.frames = [] - g.record(0.05, rate_hz=RATE_HZ, zero_gravity=True) - frames = list(fake.frames) - hand.park(0.0) - require(len(frames) > 1, - f"a zero-gravity recording streamed {len(frames)} frames in total") - hold, teaching = frames[-1], frames[:-1] - stray = [(f.kp, f.kd) for f in teaching if f.kp != 0.0 or f.kd != 0.0] - require(not stray, - f"{len(stray)} of {len(teaching)} teaching frames carried gains " - f"{stray[0] if stray else ()} — the jaws would hold position " - f"instead of following the hand") - require((hold.kp, hold.kd) == (g.config.kp, g.config.kd), - f"the frame that stopped the recording used gains " - f"({hold.kp}, {hold.kd}) instead of the configured " - f"({g.config.kp}, {g.config.kd}) — the jaws would be left slack") - return (f"{len(teaching)} teaching frames all at (kp, kd) = (0, 0), then " - f"one hold at ({hold.kp}, {hold.kd})") - - -def check_file_round_trip(traj, tmp: str) -> str: - path = traj.save("demo") - with open(path, "rb") as f: - blob = f.read() - want = 66 + 40 * len(traj) - require(blob[:8] == b"LGRTRJ01", f"magic is {blob[:8]!r}") - require(len(blob) == want, - f"{len(blob)}B for {len(traj)} samples, expected {want}B") - loaded = Trajectory.load(path) - require(loaded.samples == traj.samples, - "a sample came back different from the one written") - require(loaded.mount == traj.mount and loaded.sample_hz == traj.sample_hz, - f"provenance changed: mount {loaded.mount!r}, " - f"rate {loaded.sample_hz}") - require(close(loaded.pos_closed_rad, POS_CLOSED_RAD) - and close(loaded.pos_open_rad, POS_OPEN_RAD) - and close(loaded.rad_to_mm, RAD_TO_MM), - "the calibration the trajectory was recorded against did not " - "survive the file") - return (f"{len(blob)}B = 66B header + {len(traj)}x40B samples, reloaded " - f"into {os.path.basename(path)}") - - -def check_truncated_file_rejected(tmp: str) -> str: - """A stream that is not whole samples must not parse into half a path.""" - with open(os.path.join(tmp, "demo.lgt"), "rb") as f: - good = f.read() - cases = {"header cut off": good[:40], - "last sample cut": good[:-17], - "one stray byte": good + b"\x00"} - for label, blob in cases.items(): - broken = os.path.join(tmp, "broken.lgt") - with open(broken, "wb") as f: - f.write(blob) - try: - Trajectory.load(broken) - except TrajectoryFormatError: - continue - raise AssertionError(f"{label}: {len(blob)}B was accepted as a " - f"trajectory") - return f"{len(cases)} malformed files refused, each with a reason" - - -def check_replay_follows_the_file(g, fake, traj, show: bool = False) -> str: - """Every frame replays the trajectory's own opening at that instant.""" - frames = replay(g, fake, traj, align=False) - origin = traj.samples[0].t - require(len(frames) >= int(traj.duration / INTERVAL), - f"only {len(frames)} frames for {traj.duration:.3f}s of path at " - f"{1 / INTERVAL:.0f}Hz") - worst = 0.0 - for i, frame in enumerate(frames): - elapsed = min(i * INTERVAL, traj.duration) - worst = max(worst, abs(rad_to_openness(frame.q, g.config) - - traj.openness_at(origin + elapsed))) - require(worst < TOL, - f"a frame was off the trajectory by {worst:.3e} openness") - if show: - print(f" recorded |{bar([s.openness for s in traj.samples])}|") - print(f" replayed |{bar(commanded_openness(fake, g))}|") - return f"{len(frames)} frames at {1 / INTERVAL:.0f}Hz, worst deviation " \ - f"{worst:.1e}" - - -def check_speed_scales_the_clock(g, fake, traj) -> str: - """Speed multiplies elapsed time; a slower loop must not stretch the path.""" - slow = len(replay(g, fake, traj, speed=1.0, align=False)) - fast = len(replay(g, fake, traj, speed=2.0, align=False)) - require(abs(slow - 2 * fast) <= 3, - f"speed=2.0 sent {fast} frames against {slow} at speed=1.0: a " - f"ratio of {slow / fast:.2f}, not 2") - return f"{slow} frames at 1.0x, {fast} at 2.0x" - - -def check_align_moves_first(g, fake, traj, hand) -> str: - """``align`` puts the jaws on the opening the path starts from, first.""" - start_rad = openness_to_rad(traj.samples[0].openness, g.config) - - def frames_before_the_path(frames) -> int: - # Every frame before the path's second point commands the start opening, - # so the first frame that does not marks where following began. - return next(i for i, f in enumerate(frames) - if not close(f.q, start_rad)) - 1 - - hand.park(1.0) # jaws left fully open; the path starts closed - aligned = replay(g, fake, traj, align=True) - hand.park(1.0) - plain = replay(g, fake, traj, align=False) - - with_align = frames_before_the_path(aligned) - without = frames_before_the_path(plain) - require(without == 0, - f"align=False still moved the jaws first ({without} frames)") - require(with_align == int(ALIGN_S / INTERVAL), - f"align sent {with_align} frames before following, expected " - f"{int(ALIGN_S / INTERVAL)}") - return (f"{with_align} frames move to the start, then " - f"{len(aligned) - with_align} follow the path") - - -def check_only_position_is_replayed(traj) -> str: - """Recorded velocity and torque must never come back as feed-forward.""" - peak_torque = max(abs(s.torque_nm) for s in traj.samples) - peak_speed = max(abs(s.velocity_rad_s) for s in traj.samples) - require(peak_torque > 0.0 and peak_speed > 0.0, - "the recording's torque and velocity are all zero, so a player " - "that fed them forward would look the same as one that does not") - - g, fake = make_gripper() - FakeClock().install(g) - fake.motor.pos = openness_to_rad(traj.samples[0].openness, g.config) - frames = replay(g, fake, traj, align=False) - fed = [(f.dq, f.tau_ff) for f in frames if f.dq != 0.0 or f.tau_ff != 0.0] - require(not fed, - f"{len(fed)} of {len(frames)} frames fed recorded motion forward, " - f"e.g. (dq_target, tau_feedforward) = {fed[0] if fed else ()} — " - f"that replays force, which does not carry to a unit with another " - f"mount or calibration") - return (f"{len(frames)} frames, all pure position against a recording " - f"carrying up to {peak_torque:.2f}Nm and {peak_speed:.2f}rad/s") - - -def check_replays_on_a_reverse_mount(traj) -> str: - """The portability claim: one path, two mountings, mirrored angles.""" - def run(reverse: bool): - g, fake = make_gripper(reverse=reverse) - FakeClock().install(g) - fake.motor.pos = openness_to_rad(traj.samples[0].openness, g.config) - return g, replay(g, fake, traj, align=False) - - normal_g, normal = run(reverse=False) - reverse_g, reverse = run(reverse=True) - require(len(normal) == len(reverse), - f"{len(normal)} frames on the normal mount against {len(reverse)} " - f"on the reverse one") - - # Same opening on every frame: the recorded channel is not a raw angle. - worst = max(abs(rad_to_openness(a.q, normal_g.config) - - rad_to_openness(b.q, reverse_g.config)) - for a, b in zip(normal, reverse)) - require(worst < TOL, - f"the two mountings replayed openings differing by {worst:.3e}") - - # Mirrored angles: the two are reflections about the travel's midpoint. - midpoint = (POS_CLOSED_RAD + POS_OPEN_RAD) / 2.0 - mirror = max(abs((a.q + b.q) / 2.0 - midpoint) - for a, b in zip(normal, reverse)) - require(mirror < 1e-6, - f"the angles are off by {mirror:.3e} rad from a reflection about " - f"{midpoint:.4f}") - - # ... and the two run in opposite directions. - require((normal[1].q - normal[0].q) * (reverse[1].q - reverse[0].q) < 0.0, - "both mountings moved the same way on the first step") - return (f"{len(normal)} frames, openings within {worst:.1e}, angles " - f"mirrored about {midpoint:.4f} rad " - f"({min(f.q for f in normal):+.3f}..{max(f.q for f in normal):+.3f} " - f"vs {min(f.q for f in reverse):+.3f}.." - f"{max(f.q for f in reverse):+.3f})") - - -def check_sessions_are_exclusive(g, fake, traj) -> str: - """One long-running session at a time, and stopping always frees the slot.""" - FakeClock(real_sleep=0.002).install(g) - g.record_start(rate_hz=RATE_HZ, zero_gravity=False) - try: - status = g.trajectory_status() - require(status["kind"] == "record", - f"status says {status!r} while a recording is running") - for label, start in ( - ("a second recording", lambda: g.record_start(rate_hz=RATE_HZ)), - ("a replay", lambda: g.play_start(traj)), - ("teleoperation", - lambda: g.teleop_start("slave", - transport=InProcTeleopTransport())), - ): - try: - start() - except (TrajectoryBusyError, TeleopBusyError) as e: - require("record" in str(e), - f"{label} was refused, but the reason ({e}) does not " - f"say what is in the way") - else: - raise AssertionError(f"{label} started while one was running") - finally: - g.record_stop(allow_empty=True) - FakeClock().install(g) # hand the fast clock back to the checks - - require(g.trajectory_status()["active"] is False, - "the slot is still claimed after record_stop returned") - g.record_start(rate_hz=RATE_HZ, zero_gravity=False) - g.record_stop(allow_empty=True) - return "record, replay and teleop refused each other; the slot frees on stop" - - -def check_refusals_leave_the_slot_free(g, traj) -> str: - """The cases that must be refused are refused, without claiming the slot.""" - refused = [] - g.config.calibrated = False - try: - try: - g.record_start(rate_hz=RATE_HZ) - except TrajectoryError: - refused.append("uncalibrated recording") - else: - raise AssertionError( - "an uncalibrated unit started recording — the opening it would " - "store is a guess") - finally: - g.config.calibrated = True - - try: - g.play(Trajectory()) - except TrajectoryEmptyError: - refused.append("empty trajectory") - else: - raise AssertionError("an empty trajectory was accepted for replay") - - require(g.trajectory_status()["active"] is False, - "a refused call left the session claimed, so nothing can start " - "again") - return f"{len(refused)} refused ({', '.join(refused)}), slot still free" - - -def main() -> int: - report = Report() - with tempfile.TemporaryDirectory(prefix="litegrip-check-") as tmp: - os.environ["LITEGRIP_TRAJ_DIR"] = tmp - g, fake = make_gripper() - hand = Hand(fake, g) - FakeClock(on_tick=hand).install(g) - hand(0.0) # the hand starts where the path does - - print("Trajectory record and replay — end-to-end check, no hardware") - print(f" fake CAN on vcan0: {RATE_HZ:.0f}Hz capture, " - f"{1 / INTERVAL:.0f}Hz replay, {DURATION:.0f}s of taught motion") - - report.section("1. teach a path by hand") - traj = g.record(DURATION, rate_hz=RATE_HZ, zero_gravity=False) - print(f" hand |{bar([s.openness for s in traj.samples])}|") - report.check("the capture is the motion the hand made", - lambda: check_capture_is_the_motion(traj)) - report.check("every timestamp advances", - lambda: check_stamps_advance(traj)) - report.check("zero-gravity streams zero-torque frames", - lambda: check_zero_gravity_frames(fake, g, hand)) - - report.section("2. save and load") - report.check("a save/load round trip preserves the capture", - lambda: check_file_round_trip(traj, tmp)) - report.check("a file that is not whole samples is rejected", - lambda: check_truncated_file_rejected(tmp)) - loaded = Trajectory.load(os.path.join(tmp, "demo.lgt")) - - report.section("3. replay on the unit that recorded it") - report.check("every frame is the trajectory at that instant", - lambda: check_replay_follows_the_file(g, fake, traj, - show=True)) - report.check("speed scales elapsed time, not the frame count", - lambda: check_speed_scales_the_clock(g, fake, traj)) - report.check("align moves to the start before following", - lambda: check_align_moves_first(g, fake, traj, hand)) - report.check("only position is replayed, never force", - lambda: check_only_position_is_replayed(traj)) - - report.section("4. replay on a reverse-mounted unit") - report.check("the same path plays, mirrored in radians", - lambda: check_replays_on_a_reverse_mount(traj)) - - report.section("5. sessions and refusals") - report.check("record, replay and teleop are mutually exclusive", - lambda: check_sessions_are_exclusive(g, fake, traj)) - report.check("bad input is refused and leaves the slot free", - lambda: check_refusals_leave_the_slot_free(g, traj)) - report.check("the loaded file replays like the recording", - lambda: check_replay_follows_the_file(g, fake, loaded)) - - total = report.passed + report.failures - print(f"\n{total} checks, {report.failures} failed") - return 1 if report.failures else 0 - - -if __name__ == "__main__": - raise SystemExit(main())