diff --git a/README.md b/README.md index e0f5cc2..3419153 100644 --- a/README.md +++ b/README.md @@ -161,6 +161,72 @@ reports `active`, `mode`, `topic`, `frames`, `last_frame_age_ms`, `stale`, `open `teleop_stop()`, so its jaws hold under the configured gains. - Follow gains default to `kp=100.0`, `kd=2.0`; override with `kp=` / `kd=`. +## Trajectory record and replay + +A motion you teach by hand can be captured once and repeated later. Recording puts the motor into +zero-gravity so you can push the jaws through the motion; replay streams the captured openings back +as MIT command frames. What is stored is the normalized opening in `[0, 1]`, exactly as teleop +sends it, so a trajectory taught on one gripper replays on another with a different mount or +calibration. + +```python +from litegrip import LiteGrip + +with LiteGrip("can0") as gripper: + gripper.load_calibration() + gripper.enable() + + taught = gripper.record(5.0) # 5 s of hand-teaching; the jaws are slack + taught.save("pick") # ~/.litegrip/trajectories/pick.lgt + gripper.play(taught) # repeat it +``` + +| Method | Behaviour | +| --- | --- | +| `record(duration_s, rate_hz=100.0, zero_gravity=True)` | Blocking hand-teach. Returns the `Trajectory`. | +| `record_start(rate_hz=100.0, zero_gravity=True, max_samples=None)` | Background recording; returns the status snapshot. | +| `record_stop(allow_empty=False)` | Stops and returns the captured `Trajectory`. | +| `play(trajectory, speed=1.0, kp=None, kd=None, align=True)` | Blocking replay. `loop` must be `False`. | +| `play_start(trajectory, speed=1.0, kp=None, kd=None, loop=False, align=True)` | Background replay. | +| `play_stop(timeout=2.0)` | Stops a replay and leaves the gripper holding. | +| `trajectory_status()` | One snapshot for both directions. `active`, `kind`, `samples` and `error` are always there; a recording adds `rate_hz`, `zero_gravity` and `loop_hz`, a replay adds `frames`, `speed`, `openness` and `completed`. | + +`examples/trajectory.py` runs the same thing from the command line: + +```bash +python3 examples/trajectory.py --record 5 --save pick # hand-teach, then save +python3 examples/trajectory.py --list # no hardware needed +python3 examples/trajectory.py --play pick --repeat 3 +``` + +`Trajectory.save("pick")` writes `~/.litegrip/trajectories/pick.lgt`; a name with a path separator +in it is used as written. Set `LITEGRIP_TRAJ_DIR` to move that directory. `Trajectory.load("pick")` +reads it back, and `--list` prints one line per file. The format is compact binary with an 8-byte +magic header, and a file whose length does not match the sample count in its header is rejected +rather than parsed into half a trajectory. + +- **Replay commands position, not force.** The recorded torque is stored for diagnostics and never + fed forward, so a squeeze recorded against an object repeats as a position path that presses with + whatever `kp` yields. The grip force you taught is not preserved — follow the replay with + `grasp(force_n=...)` if it matters. +- **`record()` is exclusive and the jaws are slack for its whole duration.** It streams zero-torque + frames itself, so do not drive the gripper from the caller while it runs, and keep a hand on it: + nothing is holding the jaws. +- **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. That is the way to record a programmatic motion. +- **A capture that did not fill raises.** `record()` reports how many samples it got instead of + returning a short recording as if it were whole, and a sampling loop that died is never reported + as a good capture. +- **A blocking `play()` returns with only one hold frame sent.** The motor self-locks a + communication-loss fault about 100 ms after the frames stop, so call the next action promptly — + or use `play_start(loop=True)` with `play_stop()` for a hold that lasts. A trajectory of one + sample is a pose with nothing to repeat, so looping it holds that opening. +- **Recording, replay and teleop are mutually exclusive.** All three own the CAN I/O, and starting + a second one raises `TeleopBusyError` or `TrajectoryBusyError`. `disconnect()` stops whichever is + running. +- Record and play both require a loaded calibration: without one the normalized opening is a guess. + ## The six actions These are the supported entry points for moving the gripper. Each one verifies its own diff --git a/examples/trajectory.py b/examples/trajectory.py new file mode 100644 index 0000000..594d7b5 --- /dev/null +++ b/examples/trajectory.py @@ -0,0 +1,193 @@ +#!/usr/bin/env python3 +"""Teach a gripper a motion by hand and play it back. + +This is a runnable companion to the trajectory section of the README. + + # Hand-teach 5 seconds and save it as "pick": + python3 examples/trajectory.py --record 5 --save pick + + # List what has been saved, with no hardware attached: + python3 examples/trajectory.py --list + + # Play it back three times: + python3 examples/trajectory.py --play pick --repeat 3 + +During ``--record`` the motor goes into zero-gravity and the jaws are yours to +push: take the part, move it through the approach, the squeeze and the release, +and the samples are taken as you go. Keep a hand on the gripper — nothing is +holding the jaws while it is slack, and whatever is between them will drop. + +A saved trajectory stores the opening normalised by *this* unit's travel, so it +replays on a gripper with a different mount or calibration. What it does not +store is force: replay commands position, with the gains you give it. A squeeze +recorded against an object repeats as a position path, not as the same grip +force — use ``--kp`` and follow it with ``gripper.grasp(force_n=...)`` if the +force matters. +""" + +from __future__ import annotations + +import argparse +import sys +import time + +from litegrip import LiteGrip, LiteGripError, Trajectory, trajectory_dir + + +def build_parser() -> argparse.ArgumentParser: + parser = argparse.ArgumentParser( + description="Record a gripper motion by hand, or replay a saved one.") + action = parser.add_mutually_exclusive_group(required=True) + action.add_argument( + "--record", type=float, metavar="SECONDS", + help="hand-teach for this many seconds (the jaws go slack)") + action.add_argument( + "--play", metavar="NAME", + help="replay a saved trajectory (a bare name, or a path to a .lgt file)") + action.add_argument( + "--list", action="store_true", + help="list the saved trajectories and exit; needs no hardware") + + parser.add_argument( + "--channel", default="can0", help="CAN interface (default: can0)") + parser.add_argument( + "--can-id", type=lambda s: int(s, 0), default=0x08, + help="motor CAN ID (default: 0x08)") + parser.add_argument( + "--mount", choices=("normal", "reverse"), default=None, + help="load a mount template instead of this channel's calibration") + parser.add_argument( + "--save", metavar="NAME", + help="save the recording under this name (default: show it, save nothing)") + parser.add_argument( + "--rate", type=float, default=100.0, + help="samples per second while recording (default: 100)") + parser.add_argument( + "--speed", type=float, default=1.0, + help="playback speed multiplier; 0.5 is half speed (default: 1.0)") + parser.add_argument( + "--kp", type=float, default=None, + help="replay stiffness (default: the gripper's configured kp)") + parser.add_argument( + "--kd", type=float, default=None, + help="replay damping (default: the gripper's configured kd)") + parser.add_argument( + "--no-align", action="store_true", + help="replay: do not move to the first sample before following") + parser.add_argument( + "--repeat", type=int, default=1, + help="replay this many times (default: 1)") + parser.add_argument( + "--dry-run", action="store_true", + help="print the resolved plan and exit without touching hardware") + return parser + + +def _describe(traj: Trajectory) -> str: + return (f"{len(traj)} samples, {traj.duration:.2f}s at {traj.sample_hz:.0f}Hz, " + f"mount={traj.mount}, travel=" + f"{abs(traj.pos_open_rad - traj.pos_closed_rad) * traj.rad_to_mm:.1f}mm") + + +def _list_saved() -> int: + """Print the saved trajectories. Reads the directory, not the bus.""" + import glob + import os + + root = trajectory_dir() + paths = sorted(glob.glob(os.path.join(root, "*.lgt"))) + if not paths: + print(f"no trajectories in {root}") + return 0 + for path in paths: + try: + traj = Trajectory.load(path) + except LiteGripError as error: + # One unreadable file must not hide the rest of the list. + print(f"{os.path.basename(path)}: unreadable ({error})") + continue + print(f"{os.path.basename(path):<24} {_describe(traj)}") + return 0 + + +def _record(gripper: LiteGrip, args: argparse.Namespace) -> int: + print(f"recording {args.record}s at {args.rate}Hz — the jaws are slack now, " + f"push them through the motion") + traj = gripper.record(args.record, rate_hz=args.rate) + print(f"recorded: {_describe(traj)}") + if args.save: + written = traj.save(args.save) + print(f"saved to {written}") + else: + # Say so rather than letting the caller assume a file exists. + print("not saved (pass --save NAME to keep it)") + return 0 + + +def _play(gripper: LiteGrip, traj: Trajectory, args: argparse.Namespace) -> int: + for run in range(1, max(1, args.repeat) + 1): + status = gripper.play( + traj, speed=args.speed, kp=args.kp, kd=args.kd, + align=not args.no_align) + print(f"replay {run}/{args.repeat}: {status['frames']} frames, " + f"ended at openness {status['openness']:.3f}") + if run < args.repeat: + time.sleep(0.2) + return 0 + + +def main(argv: list[str] | None = None) -> int: + args = build_parser().parse_args(argv) + + if args.list: + return _list_saved() + + # Read the file before anything touches the bus: a mistyped name is worth + # finding out about without connecting, and the dry run should say which + # trajectory it means, not just which name was typed. + traj = None + if args.play is not None: + traj = Trajectory.load(args.play) + print(f"loaded {args.play}: {_describe(traj)}") + + gripper = LiteGrip(channel=args.channel, can_id=args.can_id) + if args.mount is not None: + gripper.load_calibration(template=args.mount) + else: + gripper.load_calibration() + print(f"mount={gripper.mount} closed={gripper.config.pos_closed_rad:+.4f} " + f"open={gripper.config.pos_open_rad:+.4f} " + f"rad_to_mm={gripper.config.rad_to_mm}") + + if args.dry_run: + what = (f"record {args.record}s at {args.rate}Hz" + if args.record is not None + else f"replay {args.play} x{args.repeat} at speed {args.speed}") + print(f"dry run: would {what} on {args.channel} " + f"(can_id=0x{args.can_id:02X}); nothing sent") + return 0 + + gripper.connect() + gripper.enable() + try: + if args.record is not None: + return _record(gripper, args) + return _play(gripper, traj, args) + finally: + # The blocking calls already hold the last position, but a Ctrl+C + # mid-call would otherwise leave the session claimed and the jaws slack. + gripper.play_stop() + gripper.disconnect() + + +if __name__ == "__main__": + try: + sys.exit(main()) + except LiteGripError as error: + print(f"error: {error}", file=sys.stderr) + sys.exit(1) + except FileNotFoundError as error: + # A mistyped --play name or --save directory; say which file, not a + # traceback the user has to read to find out. + print(f"error: no such file: {error.filename}", file=sys.stderr) + sys.exit(1) diff --git a/readme_zn.md b/readme_zn.md index 57b6f70..7609c5d 100644 --- a/readme_zn.md +++ b/readme_zn.md @@ -145,6 +145,63 @@ python3 examples/teleop.py --mode slave --channel can0 --host 0.0.0.0 - **停止后是持位**，不是卸力：主端在 `teleop_stop()` 时退出零重力模式，爪子按配置增益持位。 - 跟随增益默认 `kp=100.0`、`kd=2.0`，用 `kp=` / `kd=` 覆盖。 +## 轨迹录制与回放 + +你用手教一遍的动作可以录下来，之后反复重放。录制时电机进零重力，你直接掰爪子走完整个动作； +回放把录到的张开度按 MIT 指令帧发回去。存的是归一化到 `[0, 1]` 的张开度，和遥操线上传的是同一个 +量，所以在一台夹爪上教出来的轨迹，换一台装法不同、标定不同的夹爪也能重放。 + +```python +from litegrip import LiteGrip + +with LiteGrip("can0") as gripper: + gripper.load_calibration() + gripper.enable() + + taught = gripper.record(5.0) # 手把手教 5 秒，期间爪子是卸力的 + taught.save("pick") # ~/.litegrip/trajectories/pick.lgt + gripper.play(taught) # 重放 +``` + +| 方法 | 行为 | +| --- | --- | +| `record(duration_s, rate_hz=100.0, zero_gravity=True)` | 阻塞式手把手录制，返回 `Trajectory`。 | +| `record_start(rate_hz=100.0, zero_gravity=True, max_samples=None)` | 后台录制，返回状态快照。 | +| `record_stop(allow_empty=False)` | 停止并返回录到的 `Trajectory`。 | +| `play(trajectory, speed=1.0, kp=None, kd=None, align=True)` | 阻塞式回放。`loop` 必须是 `False`。 | +| `play_start(trajectory, speed=1.0, kp=None, kd=None, loop=False, align=True)` | 后台回放。 | +| `play_stop(timeout=2.0)` | 停止回放，并让夹爪持位。 | +| `trajectory_status()` | 两个方向共用一个快照。`active`、`kind`、`samples`、`error` 一直都在；录制时另有 `rate_hz`、`zero_gravity`、`loop_hz`，回放时另有 `frames`、`speed`、`openness`、`completed`。 | + +`examples/trajectory.py` 在命令行做同样的事： + +```bash +python3 examples/trajectory.py --record 5 --save pick # 手把手录一段再存盘 +python3 examples/trajectory.py --list # 不需要接硬件 +python3 examples/trajectory.py --play pick --repeat 3 +``` + +`Trajectory.save("pick")` 写到 `~/.litegrip/trajectories/pick.lgt`；带路径分隔符的名字按原样 +使用。目录可以用 `LITEGRIP_TRAJ_DIR` 改。`Trajectory.load("pick")` 读回来，`--list` 每个文件 +打一行。格式是紧凑二进制，开头 8 字节魔数；文件长度和头部声明的采样数对不上的会被拒绝，而不是 +解析出半截轨迹。 + +- **回放的是位置，不是力。** 录到的力矩只是诊断信息，不会前馈下发，所以对着物体挤出来的那段， + 重放时是一条位置轨迹，按 `kp` 顶上去 —— 你教的那个夹持力不会复现。力重要的话，回放完再调 + `grasp(force_n=...)`。 +- **`record()` 期间独占，而且爪子全程卸力。** 它自己持续发零力矩帧，所以录制期间不要再从调用方 + 驱动夹爪，也要用手扶着：这期间没有任何东西托着爪子。 +- **有别的东西在驱动时，用非零重力模式录。** `record_start(zero_gravity=False)` 只读状态，调用方 + 可以在另一个线程里跑 `grasp()` 或一串运动，把它录下来。要录程序化的动作就走这条路。 +- **没录满会报错。** `record()` 会说清只录到几拍，而不是把一段短录制当成完整结果返回；采样循环 + 死掉也不会被报成一次好录制。 +- **阻塞式 `play()` 返回时只发了一帧持位。** 电机在停帧约 100 ms 后会因通信丢失自锁，所以要接着 + 调下一个动作 —— 想要持续持位就用 `play_start(loop=True)` 配 `play_stop()`。只有一拍采样的轨迹 + 是一个姿势、没有可循环的行程，循环它就等于一直保持那个张开度。 +- **录制、回放、遥操三者互斥。** 它们都独占 CAN 读写，起第二个会抛 `TeleopBusyError` 或 + `TrajectoryBusyError`。`disconnect()` 会把正在跑的那个停掉。 +- 录制和回放都要求已加载标定：没有标定，归一化的张开度算不出来。 + ## 六个动作接口 要让夹爪动起来就用这六个。每一个都会自己校验结果再报成功，所以调用方不必再重写斜坡和 diff --git a/src/litegrip/__init__.py b/src/litegrip/__init__.py index 232a1a4..aa2249f 100644 --- a/src/litegrip/__init__.py +++ b/src/litegrip/__init__.py @@ -123,6 +123,23 @@ def _detect_version(dist_name: str = "litegrip") -> str: teleop_topic, ) +# ── Trajectory record and replay ──────────────────────────────────────── +from .trajectory import ( + Trajectory, + TrajectorySample, + TrajectoryRecorder, + TrajectoryPlayer, + trajectory_dir, + resolve_path, + DEFAULT_RATE_HZ, + TrajectoryError, + TrajectoryBusyError, + TrajectoryNotActiveError, + TrajectoryEmptyError, + TrajectoryRecordingError, + TrajectoryFormatError, +) + # ── CAN subpackage (expert) ───────────────────────────────────────────── from . import can @@ -179,6 +196,20 @@ def _detect_version(dist_name: str = "litegrip") -> str: "encode_frame", "decode_frame", "teleop_topic", + # Trajectory record and replay + "Trajectory", + "TrajectorySample", + "TrajectoryRecorder", + "TrajectoryPlayer", + "trajectory_dir", + "resolve_path", + "DEFAULT_RATE_HZ", + "TrajectoryError", + "TrajectoryBusyError", + "TrajectoryNotActiveError", + "TrajectoryEmptyError", + "TrajectoryRecordingError", + "TrajectoryFormatError", # Subpackages "can", ] diff --git a/src/litegrip/gripper.py b/src/litegrip/gripper.py index f687067..97a460f 100644 --- a/src/litegrip/gripper.py +++ b/src/litegrip/gripper.py @@ -230,6 +230,17 @@ def __init__( # injected), so teleop_stop knows what it is allowed to close. self._teleop_transport: Optional["TeleopTransport"] = None + # One long-running session at a time. Teleoperation and a trajectory + # both own the CAN I/O for their whole duration and both run in their + # own thread, so "is teleop running? no → start recording" is a + # check-then-act race that lets two loops interleave frames on one + # motor. Claiming and releasing under one lock closes it. The owner + # string names the session, so the refusal says who is in the way. + self._session_lock = threading.Lock() + self._session_owner: Optional[str] = None + self._trajectory_recorder: Optional["TrajectoryRecorder"] = None + self._trajectory_player: Optional["TrajectoryPlayer"] = None + # Declaring the mount is just loading the matching template, so it # costs no CAN traffic and is safe this early. The template carries # only a direction and geometry, so it cannot clobber the identity @@ -347,6 +358,17 @@ def disconnect(self) -> None: if self._teleop is not None: self.teleop_stop() + # A recording left running would keep sampling a transport that is + # about to close, and a replay would keep commanding one. stop() also + # leaves the gripper holding rather than slack. + if self._trajectory_recorder is not None: + self._trajectory_recorder.stop() + self._release_session("record") + self._trajectory_recorder = None + if self._trajectory_player is not None: + self._trajectory_player.stop() + self._release_session("play") + self._trajectory_player = None if self._can: self._can.disconnect(disable=self._disable_on_disconnect) @@ -535,10 +557,28 @@ def enter_zero_gravity(self, duration: float = 0.0) -> None: def exit_zero_gravity(self) -> None: """Exit zero-gravity mode and hold current position.""" if self._can is not None and self._enabled: + self._hold_position() + log.info("Zero-gravity mode exited; holding position") + + def _hold_position(self) -> None: + """Send one MIT frame holding the current position under the configured gains. + + The one "hold, do not go slack" primitive in this SDK: the teleop + master and a stopping trajectory replay both need it, and neither wants + zero-gravity mode's log line. It is one frame, not a sustained hold — + the motor self-locks a communication-loss fault about 100 ms after the + frames stop, so a caller that needs the jaws held longer has to keep + sending (or call a motion action). + """ + if self._can is None or not self._enabled: + return + # Under the same lock as every other CAN operation: a caller normally + # joins its loop before holding, but a join that times out would + # otherwise let this frame overtake one still being written. + with self._io_lock: current_pos = self._can.get_position() self._can.control_mit(q_target=current_pos, kp=self._config.kp, kd=self._config.kd, tau_feedforward=0) - log.info("Zero-gravity mode exited; holding position") # ═══════════════════════════════════════════════════════════════════ # Manual calibration (zero-gravity assisted) @@ -1741,21 +1781,26 @@ def teleop_start( raise ValueError(f"mode must be 'master' or 'slave', got {mode!r}") if self._teleop is not None and self._teleop.is_running: raise TeleopBusyError("teleop is already running") + self._claim_session("teleop", TeleopBusyError) - created_transport = None - if transport is None: - if host is None: - raise ValueError("host is required when no transport is given") - addr = f"{host}:{port}" - if mode == "master": - transport = created_transport = UdpTeleopTransport(pub_addr=addr) - else: - transport = created_transport = UdpTeleopTransport(bind_addr=addr) - - manager = GripperTeleop( - self, transport, mode, teleop_topic(master_id), - rate_hz=rate_hz, kp=kp, kd=kd, align=align, watchdog_s=watchdog_s) - manager.start() + try: + created_transport = None + if transport is None: + if host is None: + raise ValueError("host is required when no transport is given") + addr = f"{host}:{port}" + if mode == "master": + transport = created_transport = UdpTeleopTransport(pub_addr=addr) + else: + transport = created_transport = UdpTeleopTransport(bind_addr=addr) + + manager = GripperTeleop( + self, transport, mode, teleop_topic(master_id), + rate_hz=rate_hz, kp=kp, kd=kd, align=align, watchdog_s=watchdog_s) + manager.start() + except BaseException: + self._release_session("teleop") + raise self._teleop = manager self._teleop_transport = created_transport return manager.status() @@ -1771,6 +1816,7 @@ def teleop_stop(self, timeout: float = 2.0) -> dict: return {"active": False, "mode": None} manager.stop(timeout=timeout) self._teleop = None + self._release_session("teleop") if self._teleop_transport is not None: try: self._teleop_transport.close() @@ -1785,6 +1831,295 @@ def teleop_status(self) -> dict: return {"active": False, "mode": None} return self._teleop.status() + # ═══════════════════════════════════════════════════════════════════ + # Trajectory record and replay + # ═══════════════════════════════════════════════════════════════════ + # + # Teach a motion once, repeat it later. See litegrip.trajectory for what + # the recording stores and, importantly, what a replay does *not* + # reproduce (force). + + def record_start( + self, + rate_hz: float = 100.0, + zero_gravity: bool = True, + max_samples: Optional[int] = None, + ) -> dict: + """Begin recording this gripper's motion, in the background. + + With ``zero_gravity=True`` — the hand-teaching mode — the recorder + itself streams zero-torque frames so the jaws can be pushed by hand. + Do not drive the gripper from the caller while that runs. With + ``zero_gravity=False`` the recorder only *reads* state, so the caller is + free to drive the gripper from another thread and capture a + programmatic move; only the sampling touches the bus. + + Recording is exclusive with teleoperation and replay. + + Args: + rate_hz: Samples per second (default 100). + zero_gravity: Stream zero-torque frames, leaving the jaws + back-drivable by hand. + max_samples: Stop by itself after this many samples; ``None`` + records until :meth:`record_stop`. + + Returns: + The initial :meth:`trajectory_status` snapshot. + + Raises: + TrajectoryBusyError: another session (teleop, record, play) is running. + NotInitializedError: not connected or not enabled. + TrajectoryError: the unit is not calibrated, so the normalised + opening a sample stores would be a guess. + """ + from .trajectory import (TrajectoryBusyError, TrajectoryError, + TrajectoryRecorder) + + self._check_connected() + self._check_enabled() + if not self._config.calibrated: + raise TrajectoryError( + "未标定 —— 轨迹记录的是按行程归一化的张开度, 没有标定就算不出来; " + "先 load_calibration() 或 zero()") + self._claim_session("record", TrajectoryBusyError) + try: + recorder = TrajectoryRecorder( + self, rate_hz=rate_hz, zero_gravity=zero_gravity, + max_samples=max_samples) + recorder.start() + except BaseException: + self._release_session("record") + raise + self._trajectory_recorder = recorder + return recorder.status() + + def record_stop(self, allow_empty: bool = False) -> "Trajectory": + """Stop recording and return the captured trajectory. + + Args: + allow_empty: Return an empty trajectory instead of raising when + nothing was captured. For a deliberate start-then-immediately- + stop; a capture that was *meant* to contain motion should be + allowed to raise. + + Returns: + The recorded :class:`~litegrip.Trajectory`. + + Raises: + TrajectoryNotActiveError: nothing is being recorded. + TrajectoryRecordingError: the sampling loop died — a partial + capture is never returned as if it were whole. + TrajectoryEmptyError: no samples were captured. + """ + from .trajectory import TrajectoryNotActiveError + + recorder = self._trajectory_recorder + if recorder is None: + raise TrajectoryNotActiveError("没有正在进行的录制") + recorder.stop() + self._trajectory_recorder = None + self._release_session("record") + return recorder.result(allow_empty=allow_empty) + + def record( + self, + duration_s: float, + rate_hz: float = 100.0, + zero_gravity: bool = True, + ) -> "Trajectory": + """Hand-teach a motion: record for *duration_s* seconds, return it. + + Blocking. Zero-gravity is on, so the jaws go slack and you push them + through the motion by hand while the samples are taken. The call + returns once the capture is complete; if it did not fill, it raises and + says how many samples it got rather than returning a short recording. + + Args: + duration_s: How many seconds to record. The gripper is slack for + that long — keep a hand on it, and be aware that the jaws hold + nothing while it is slack. + rate_hz: Samples per second (default 100). + zero_gravity: ``False`` only if something else drives the gripper + during the recording — see :meth:`record_start`. + + Raises: + ValueError: ``duration_s <= 0``. + TrajectoryBusyError: another session is running. + NotInitializedError: not connected or not enabled. + TrajectoryError: the unit is not calibrated. + TrajectoryRecordingError: the capture did not fill. + + Example:: + + with LiteGrip("can0") as gripper: + gripper.load_calibration() + gripper.enable() + taught = gripper.record(5.0) # push the jaws by hand + taught.save("pick") # ~/.litegrip/trajectories/pick.lgt + gripper.play(taught) # repeat it + """ + duration_s = float(duration_s) + if duration_s <= 0.0: + raise ValueError(f"duration_s 需 > 0 (给的是 {duration_s})") + target = max(1, int(round(duration_s * float(rate_hz)))) + + self.record_start(rate_hz=rate_hz, zero_gravity=zero_gravity, + max_samples=target) + recorder = self._trajectory_recorder + try: + recorder.wait_for(target, timeout=duration_s * 1.5 + 3.0) + except BaseException: + # Never leave the jaws slack and the session claimed because the + # capture went wrong — clean up, then let the error through. + recorder.stop() + self._trajectory_recorder = None + self._release_session("record") + raise + return self.record_stop() + + def play_start( + self, + trajectory: "Trajectory", + speed: float = 1.0, + kp: Optional[float] = None, + kd: Optional[float] = None, + loop: bool = False, + align: bool = True, + ) -> dict: + """Replay a trajectory in the background. + + Only position is replayed: the recorded torque and velocity are + diagnostics, never feed-forward, so a motion recorded while gripping an + object replays as a position path and *not* as the same gripping force. + Follow it with :meth:`grasp` if the force matters. + + Args: + trajectory: A :class:`~litegrip.Trajectory`, from :meth:`record_stop` + or :meth:`~litegrip.Trajectory.load`. + speed: Timing multiplier; ``0.5`` plays at half speed. + kp, kd: Gains for the replay frames; ``None`` uses the configured ones. + loop: Restart at the end instead of stopping. A one-sample + trajectory is a pose, so looping it holds that opening. + align: Move to the trajectory's first opening before following, so + the first frame is not a step from wherever the jaws are. + + Returns: + The initial :meth:`trajectory_status` snapshot. + + Raises: + TrajectoryBusyError: another session is running. + TrajectoryEmptyError: the trajectory has no samples. + ValueError: ``speed <= 0``. + NotInitializedError: not connected or not enabled. + TrajectoryError: the unit is not calibrated — the opening has to be + converted back through *this* gripper's travel, and with the + placeholder limits that conversion is a guess. + """ + from .trajectory import (TrajectoryBusyError, TrajectoryError, + TrajectoryPlayer) + + self._check_connected() + self._check_enabled() + if not self._config.calibrated: + raise TrajectoryError( + "未标定 —— 回放要把张开度按本机行程换算回角度, 没有标定就是瞎走; " + "先 load_calibration() 或 zero()") + self._claim_session("play", TrajectoryBusyError) + try: + player = TrajectoryPlayer( + self, trajectory, speed=speed, kp=kp, kd=kd, loop=loop, + align=align) + player.start() + except BaseException: + self._release_session("play") + raise + self._trajectory_player = player + return player.status() + + def play( + self, + trajectory: "Trajectory", + speed: float = 1.0, + kp: Optional[float] = None, + kd: Optional[float] = None, + loop: bool = False, + align: bool = True, + ) -> dict: + """Replay a trajectory once, blocking until it finishes. + + The last frame holds the final position under the configured gains, but + it is *one* frame: the motor self-locks a communication-loss fault about + 100 ms after the frames stop. Call the next action promptly, or use + :meth:`play_start` with ``loop=True`` for a hold that lasts until + :meth:`play_stop`. + + Args: + loop: Must be ``False``. A blocking replay of a looping trajectory + never returns; use :meth:`play_start` for that. + + Returns: + The final :meth:`trajectory_status` snapshot. + + Raises: + ValueError: ``loop`` is true, or ``speed <= 0``. + TrajectoryBusyError: another session is running. + TrajectoryEmptyError: the trajectory has no samples. + TrajectoryError: the unit is not calibrated, or the replay stopped + early — a send failed, or the sampling clock stalled. + NotInitializedError: not connected or not enabled. + """ + from .trajectory import TrajectoryError + + if loop: + raise ValueError( + "loop=True 的阻塞回放永远不会返回; 要循环播放用 " + "play_start(loop=True), 再用 play_stop() 停") + self.play_start(trajectory, speed=speed, kp=kp, kd=kd, loop=False, + align=align) + player = self._trajectory_player + # Wall-clock pacing plus one align move; the margin covers a slow first + # frame. A stall guard of its own, so a stopped clock cannot hang here. + budget = abs(float(trajectory.duration)) / float(speed) * 1.5 + 4.0 + try: + finished = player.wait(budget) + except BaseException: + # Ctrl+C during a long replay is the ordinary way out of this call. + # Without the stop the player keeps commanding the motor and the + # session stays claimed, so every later record/play is refused as + # busy until the process ends. + self.play_stop() + raise + status = self.play_stop() + if not finished: + raise TrajectoryError( + f"回放未在 {budget:.1f}s 内结束 (已发 {status.get('frames', 0)} 帧) " + f"—— 采样时钟可能停住了") + if status.get("error") is not None: + raise TrajectoryError(f"回放中止: {status['error']}") + return status + + def play_stop(self, timeout: float = 2.0) -> dict: + """Stop a replay and leave the gripper holding its last target.""" + player = self._trajectory_player + if player is None: + return {"active": False, "kind": None} + player.stop(timeout=timeout) + self._trajectory_player = None + self._release_session("play") + return player.status() + + def trajectory_status(self) -> dict: + """Snapshot of the running recording or replay, or ``{"active": False}``. + + One call for both directions: the ``kind`` key says which + (``"record"`` / ``"play"``), and only one of them can be running. + """ + if self._trajectory_recorder is not None: + return self._trajectory_recorder.status() + if self._trajectory_player is not None: + return self._trajectory_player.status() + return {"active": False, "kind": None} + # ═══════════════════════════════════════════════════════════════════ # Internal # ═══════════════════════════════════════════════════════════════════ @@ -1797,6 +2132,34 @@ def _check_enabled(self) -> None: if not self._enabled: raise NotInitializedError("未使能 — 请先调用 enable()") + def _claim_session(self, owner: str, error: type) -> None: + """Take the single long-running session slot, or raise *error*. + + Teleoperation, trajectory recording and trajectory replay all own the + CAN I/O for their duration. Claiming happens under one lock, so two + threads starting different sessions at the same instant cannot both + pass the check and then both start. + """ + with self._session_lock: + if self._session_owner is not None: + raise error( + f"已有会话在运行 ({self._session_owner}) —— " + f"teleop、录制、回放同一时刻只能有一个") + self._session_owner = owner + + def _release_session(self, owner: str) -> None: + """Give the session slot back. Harmless if *owner* no longer holds it.""" + with self._session_lock: + if self._session_owner == owner: + self._session_owner = None + + @property + def session(self) -> Optional[str]: + """Which long-running session is active: ``None``, ``"teleop"``, + ``"record"`` or ``"play"``. For diagnostics and for a UI that has to + disable the controls that cannot run at the same time.""" + return self._session_owner + # ═══════════════════════════════════════════════════════════════════ # Context manager # ═══════════════════════════════════════════════════════════════════ diff --git a/src/litegrip/trajectory.py b/src/litegrip/trajectory.py new file mode 100644 index 0000000..7a5bac1 --- /dev/null +++ b/src/litegrip/trajectory.py @@ -0,0 +1,918 @@ +"""Trajectory record and replay — teach the jaws a motion once, repeat it later. + +Read this if you want to capture a motion off a real gripper and play it back: +the approach path that seats a part, the wiggle that shakes it loose, the squeeze +profile you found by hand. ``gripper.record(...)`` puts the motor into +zero-gravity, lets you push the jaws through the motion, and samples what they +did; ``gripper.play(trajectory)`` streams it back as MIT command frames. + +Two halves, two classes: + +* :class:`TrajectoryRecorder` — a background sampling loop. +* :class:`TrajectoryPlayer` — a background command loop. + +and :class:`Trajectory` / :class:`TrajectorySample` — the captured data and its +on-disk format. + +Why the recording stores ``openness`` and not radians +---------------------------------------------------- +Each gripper has its own zero, direction and calibration (one unit opens toward +-1.42 rad, another toward +1.14 rad), so a raw angle is meaningless on a +different unit. A sample therefore carries the opening normalised by the +*recording* unit's travel — dimensionless, direction-free — and replay converts +it back with the *local* calibration. A trajectory taught on a normal-mount +gripper replays correctly on a reverse-mounted one. The recorded angle, +velocity and torque are kept as diagnostics only. + +What replay does not reproduce +------------------------------ +Replay commands **position**, not force. The opening is clamped to the local +calibrated travel, and the recorded torque is never fed forward, so a squeeze +that was recorded against an object replays as a position path that presses +with whatever ``kp`` yields — the grasp force you taught is *not* preserved. +For a repeatable grip force, replay the motion and then call +:meth:`~litegrip.LiteGrip.grasp` with an explicit ``force_n``. + +Traps this module works around +------------------------------ +* The DM motor self-locks a communication-loss fault roughly 100 ms after the + frames stop. A recorder that only *sampled* would therefore fault the motor + it is teaching, so zero-gravity recording streams a zero-torque frame every + cycle — the same rule as the teleop master loop. +* A blocking :meth:`~litegrip.LiteGrip.record` returns only once the capture is + complete; a capture that did not fill raises and reports how many samples + landed, rather than handing back a short recording as if it were whole. +* A file whose length does not match the sample count in its header is rejected, + not parsed into half a trajectory. +* A sample whose timestamp did not advance is not stored, and a clock that will + not advance aborts either loop — rather than filling a "successful" recording + with rows that all claim the same instant, or replaying forever. +""" + +from __future__ import annotations + +import logging +import os +import struct +import threading +import time +from dataclasses import dataclass, field +from typing import Any, Callable, List, Optional, Tuple + +from .exceptions import LiteGripError +from .teleop import openness_to_rad, rad_to_openness + +log = logging.getLogger("litegrip.trajectory") + +__all__ = [ + "TrajectorySample", + "Trajectory", + "TrajectoryRecorder", + "TrajectoryPlayer", + "trajectory_dir", + "TrajectoryError", + "TrajectoryBusyError", + "TrajectoryNotActiveError", + "TrajectoryEmptyError", + "TrajectoryRecordingError", + "TrajectoryFormatError", + "DEFAULT_RATE_HZ", +] + +#: Default sampling rate for a hand-taught recording. Fast enough to keep the +#: shape of a motion a human made, slow enough that a CAN round trip per sample +#: always fits: 100 Hz means a 10 ms budget, and a status frame takes ~1 ms. +DEFAULT_RATE_HZ = 100.0 + +#: Two clock readings closer together than this count as "the clock did not +#: advance" — see :data:`_STALL_CYCLES`. One nanosecond, because the target +#: platform is Linux, where ``time.monotonic`` has nanosecond resolution: a +#: running loop always moves further than this between two cycles, and a +#: stopped clock never moves at all. +_CLOCK_EPS = 1e-9 + +#: How many consecutive non-advancing cycles abort a sampling or replay loop. +#: A real monotonic clock never does this — five cycles of Python and a CAN +#: round trip cannot land inside one nanosecond — so it only ever fires on a +#: clock that has stopped, where without the guard the loop would spin forever. +_STALL_CYCLES = 5 + + +# ── file format ─────────────────────────────────────────────────────────── +# +# Little-endian throughout, matching the fixed-layout samples litearm-core +# writes. (litegrip's teleop wire frames are big-endian for byte compatibility +# with the litearm implementation; nothing outside this SDK reads this file, so +# there is no interop to preserve and the native order wins.) +# +# header magic 8s | version u16 | n_samples u32 | sample_hz f64 | +# created f64 | pos_closed_rad f64 | pos_open_rad f64 | +# rad_to_mm f64 | can_id u32 | mount 8s = 66 B +# sample t f64 | openness f64 | position_rad f64 | velocity_rad_s f64 | +# torque_nm f64 = 40 B + +_MAGIC = b"LGRTRJ01" +_VERSION = 1 +_HEADER = struct.Struct("<8sHI5dI8s") +_SAMPLE = struct.Struct("<5d") +_MOUNT_FIELD = 8 +_MOUNTS = (None, "normal", "reverse") + + +def trajectory_dir() -> str: + """Directory a trajectory is saved to when only a name is given. + + ``~/.litegrip/trajectories`` — next to the per-channel calibration files + (see :func:`~litegrip.default_calib_path`), because a trajectory belongs to + the machine, not to the working directory the program happened to start in. + ``LITEGRIP_TRAJ_DIR`` overrides it, for tests and for a controller with a + read-only home. + """ + env = os.environ.get("LITEGRIP_TRAJ_DIR") + if env: + return env + return os.path.join(os.path.expanduser("~"), ".litegrip", "trajectories") + + +def resolve_path(path: str) -> str: + """A bare name lands in :func:`trajectory_dir`; anything else is a path. + + ``"pick"`` → ``~/.litegrip/trajectories/pick.lgt``; ``"out/pick.lgt"`` and + ``"/tmp/pick.lgt"`` are used as written. + """ + if os.sep in path or (os.altsep and os.altsep in path): + return path + name = path if path.endswith(".lgt") else path + ".lgt" + return os.path.join(trajectory_dir(), name) + + +# ── exceptions ──────────────────────────────────────────────────────────── + + +class TrajectoryError(LiteGripError): + """Base class for trajectory record/replay errors.""" + + +class TrajectoryBusyError(TrajectoryError): + """Raised when a recording or replay is started while one is already running.""" + + +class TrajectoryNotActiveError(TrajectoryError): + """Raised when a stop is asked for but nothing is running.""" + + +class TrajectoryEmptyError(TrajectoryError): + """Raised when a capture or a replay would deal with zero samples.""" + + +class TrajectoryRecordingError(TrajectoryError): + """Raised when the recording loop died, or a timed capture did not fill.""" + + +class TrajectoryFormatError(TrajectoryError): + """Raised when a byte stream is not a well-formed trajectory file.""" + + +# ── data ────────────────────────────────────────────────────────────────── + + +@dataclass(frozen=True) +class TrajectorySample: + """One sample of a recorded motion. + + Attributes: + t: Seconds since the recording started. A trimmed clock reading of the + *recording* machine — it means nothing after a reboot or on another + host, and replay only ever uses the differences between samples. + openness: Opening in ``[0, 1]`` (0 = closed, 1 = fully open). The + channel replay actually follows; see the module docstring. + position_rad: Raw motor angle at that instant — diagnostic. Only + meaningful against the calibration recorded alongside it. + velocity_rad_s: Motor velocity — diagnostic. + torque_nm: Motor torque — diagnostic. Also the closest thing to a + force record, but replay does not feed it forward. + """ + + t: float + openness: float + position_rad: float + velocity_rad_s: float = 0.0 + torque_nm: float = 0.0 + + +@dataclass +class Trajectory: + """A recorded motion: samples plus the calibration they were taken against. + + The geometry fields describe the gripper that *recorded* the trajectory and + travel with it in the file, so a loaded trajectory still reports which unit + it came from and how its ``openness`` values were derived. Replay ignores + them in favour of the local gripper's own calibration. + """ + + samples: List[TrajectorySample] = field(default_factory=list) + sample_hz: float = DEFAULT_RATE_HZ + created: float = field(default_factory=time.time) + can_id: int = 0x08 + pos_closed_rad: float = 0.0 + pos_open_rad: float = 0.0 + rad_to_mm: float = 0.0 + mount: Optional[str] = None + + def __len__(self) -> int: + return len(self.samples) + + @property + def duration(self) -> float: + """Seconds from the first sample to the last (0.0 for a short one). + + The span, not the last sample's timestamp: a trajectory whose first + sample does not sit at ``t = 0`` still lasts only as long as its own + samples cover, and reporting the raw end stamp would make replay hold + its opening for the whole offset before starting to move. + """ + if not self.samples: + return 0.0 + return float(self.samples[-1].t - self.samples[0].t) + + def openness_at(self, t: float) -> float: + """Opening at time *t*, linearly interpolated between samples. + + Clamped at both ends: before the first sample and after the last one the + nearest sample's opening is returned. Signals that move are sampled far + faster than they move, so linear interpolation between neighbours is + well below the mechanical resolution — a smoother curve would be + inventing detail the recording does not contain. + """ + samples = self.samples + if not samples: + raise TrajectoryEmptyError("轨迹没有采样点") + if t <= samples[0].t: + return samples[0].openness + if t >= samples[-1].t: + return samples[-1].openness + lo, hi = 0, len(samples) - 1 + while hi - lo > 1: # bisect on t + mid = (lo + hi) // 2 + if samples[mid].t <= t: + lo = mid + else: + hi = mid + a, b = samples[lo], samples[hi] + span = b.t - a.t + if span <= 0.0: + return b.openness + return a.openness + (t - a.t) / span * (b.openness - a.openness) + + # ── serialisation ───────────────────────────────────────────────── + + def to_bytes(self) -> bytes: + """Serialise to the binary format described in this module's header. + + Raises: + TrajectoryError: The mount name does not fit its field. + """ + mount = (self.mount or "").encode("ascii") + if len(mount) > _MOUNT_FIELD: + raise TrajectoryError( + f"mount 名 {self.mount!r} 超过 {_MOUNT_FIELD} 字节") + parts = [_HEADER.pack( + _MAGIC, _VERSION, len(self.samples), float(self.sample_hz), + float(self.created), float(self.pos_closed_rad), + float(self.pos_open_rad), float(self.rad_to_mm), int(self.can_id), + mount.ljust(_MOUNT_FIELD, b"\x00"))] + for s in self.samples: + parts.append(_SAMPLE.pack( + float(s.t), float(s.openness), float(s.position_rad), + float(s.velocity_rad_s), float(s.torque_nm))) + return b"".join(parts) + + @classmethod + def from_bytes(cls, blob: bytes) -> "Trajectory": + """Parse a trajectory file, or say precisely why it is not one. + + Every check here exists to stop a corrupt file from becoming a + plausible-looking motion: the header's sample count has to match the + payload exactly (a truncated download, a half-written file and a file + with another file's tail appended are all caught by the same rule), and + the samples have to be finite, in range and ordered in time. + + Raises: + TrajectoryFormatError: Magic, version, length, header fields or + sample values are not valid. + """ + blob = bytes(blob) + if len(blob) < _HEADER.size: + raise TrajectoryFormatError( + f"文件只有 {len(blob)}B, 连 {_HEADER.size}B 的头都不够") + (magic, version, n, hz, created, pos_closed, pos_open, rad_to_mm, + can_id, mount_raw) = _HEADER.unpack_from(blob, 0) + if magic != _MAGIC: + raise TrajectoryFormatError( + f"magic 不对: {magic!r} (期望 {_MAGIC!r}) —— 不是轨迹文件") + if version != _VERSION: + raise TrajectoryFormatError( + f"格式版本 {version} 不是本 SDK 能读的版本 {_VERSION}") + want = _HEADER.size + n * _SAMPLE.size + if len(blob) != want: + raise TrajectoryFormatError( + f"文件长度 {len(blob)}B 与头部声明的 {n} 拍 ({want}B) 不符 " + f"(差 {len(blob) - want:+d}B) —— 文件被截断或尾部多了数据, " + f"拒绝解析出半截轨迹") + if not (hz > 0.0): + raise TrajectoryFormatError(f"采样率非法: {hz}") + if not (rad_to_mm > 0.0): + raise TrajectoryFormatError(f"rad_to_mm 非法: {rad_to_mm}") + mount = mount_raw.split(b"\x00", 1)[0].decode("ascii", "replace") + if mount not in ("", "normal", "reverse"): + raise TrajectoryFormatError(f"装法名非法: {mount!r}") + if not (abs(pos_open - pos_closed) * rad_to_mm > 0.0): + raise TrajectoryFormatError( + f"行程为零 (closed={pos_closed}, open={pos_open})") + + samples: List[TrajectorySample] = [] + last_t = float("-inf") + for i in range(n): + values = _SAMPLE.unpack_from(blob, _HEADER.size + i * _SAMPLE.size) + t, openness, position_rad, velocity, torque = values + for name, value in zip( + ("t", "openness", "position_rad", "velocity_rad_s", + "torque_nm"), values): + if value != value or value in (float("inf"), float("-inf")): + raise TrajectoryFormatError( + f"第 {i} 拍的 {name} 不是有限数: {value}") + if not 0.0 <= openness <= 1.0: + raise TrajectoryFormatError( + f"第 {i} 拍的 openness={openness} 不在 [0, 1] 内") + if t < last_t: + raise TrajectoryFormatError( + f"第 {i} 拍的 t={t} 比上一拍 {last_t} 还早 —— 时间未单调") + last_t = t + samples.append(TrajectorySample( + t=t, openness=openness, position_rad=position_rad, + velocity_rad_s=velocity, torque_nm=torque)) + + return cls(samples=samples, sample_hz=hz, created=created, can_id=can_id, + pos_closed_rad=pos_closed, pos_open_rad=pos_open, + rad_to_mm=rad_to_mm, mount=mount or None) + + def save(self, path: str) -> str: + """Write to *path*, creating the parent directory. Returns the path. + + A bare name goes to :func:`trajectory_dir` (see :func:`resolve_path`). + """ + target = resolve_path(path) + parent = os.path.dirname(target) + if parent: + os.makedirs(parent, exist_ok=True) + with open(target, "wb") as f: + f.write(self.to_bytes()) + log.info("trajectory saved: %s (%d samples)", target, len(self.samples)) + return target + + @classmethod + def load(cls, path: str) -> "Trajectory": + """Read a trajectory written by :meth:`save`.""" + with open(resolve_path(path), "rb") as f: + return cls.from_bytes(f.read()) + + +# ── loop pacing ─────────────────────────────────────────────────────────── + + +class _Pacer: + """Frame pacing and loop-rate measurement, shared by both loops. + + ``rest()`` sleeps whatever is left of a cycle's budget, so the loop holds + its rate without drifting when a cycle overruns. ``loop_hz`` is the + measured rate over the last second — the number to look at when a recording + sounds wrong, because a loop that cannot keep up drops samples rather than + stretching time. + """ + + def __init__(self, dt: float, sleep_fn: Callable[[float], None], + monotonic_fn: Callable[[], float]) -> None: + self._dt = dt + self._sleep_fn = sleep_fn + self._monotonic_fn = monotonic_fn + self._loops = 0 + self._hz_t0 = 0.0 + self._hz_n0 = 0 + self.loop_hz = 0.0 + + def rest(self, t0: float) -> None: + self._loops += 1 + now = self._monotonic_fn() + if self._hz_t0 == 0.0: + self._hz_t0 = now + self._hz_n0 = self._loops + elif now - self._hz_t0 >= 1.0: + self.loop_hz = (self._loops - self._hz_n0) / (now - self._hz_t0) + self._hz_t0 = now + self._hz_n0 = self._loops + rest = self._dt - (self._monotonic_fn() - t0) + if rest > 0.0: + self._sleep_fn(rest) + + +def _resolve_seams( + gripper: Any, + sleep_fn: Optional[Callable[[float], None]], + monotonic_fn: Optional[Callable[[], float]], +) -> Tuple[Callable[[float], None], Callable[[], float]]: + """Fill in the timing seams from the gripper's :class:`MotionConfig`. + + The motion engine already takes ``sleep_fn`` / ``monotonic_fn`` from there, + and the fixture every test builds its gripper with stubs them. Defaulting + to the same place is what lets a test drive a whole recording without + stubbing this module — and letting a caller pass them explicitly is what + keeps the classes usable on their own. + """ + cfg = getattr(gripper, "motion_config", None) + if sleep_fn is None: + sleep_fn = getattr(cfg, "sleep_fn", None) or time.sleep + if monotonic_fn is None: + monotonic_fn = getattr(cfg, "monotonic_fn", None) or time.monotonic + return sleep_fn, monotonic_fn + + +# ── recording ───────────────────────────────────────────────────────────── + + +class TrajectoryRecorder: + """Samples the gripper's state into a :class:`Trajectory`, on a thread. + + With ``zero_gravity=True`` the loop streams ``q=0, kp=0, kd=0`` every cycle, + which leaves the motor energised but torque-free so the jaws can be pushed + by hand — that is the hand-teaching mode, and the zero-torque frame is + mandatory, not a convenience (see the module docstring). With + ``zero_gravity=False`` the loop only *reads*, so the caller is free to drive + the gripper from another thread while it records: that is how a + programmatic ``grasp()`` or a move sequence gets captured. + + Args: + gripper: The ``LiteGrip`` to sample. + rate_hz: Samples per second. ``DEFAULT_RATE_HZ`` (100) unless the + motion is fast. + zero_gravity: Stream zero-torque frames so the jaws can be hand-driven. + max_samples: Stop by itself after this many samples. ``None`` records + until :meth:`stop`. Bounded recordings are what make a stuck clock + visible as a timeout instead of a running process. + sleep_fn, monotonic_fn: Timing seams for tests, matching + :class:`~litegrip.MotionConfig`'s names. + """ + + def __init__( + self, + gripper: Any, + rate_hz: float = DEFAULT_RATE_HZ, + zero_gravity: bool = True, + max_samples: Optional[int] = None, + sleep_fn: Optional[Callable[[float], None]] = None, + monotonic_fn: Optional[Callable[[], float]] = None, + ) -> None: + if rate_hz <= 0.0: + raise ValueError(f"rate_hz must be > 0, got {rate_hz!r}") + self._g = gripper + self._rate_hz = float(rate_hz) + self._zero_gravity = bool(zero_gravity) + self._max_samples = None if max_samples is None else int(max_samples) + self._sleep_fn, self._monotonic_fn = _resolve_seams( + gripper, sleep_fn, monotonic_fn) + self._pacer = _Pacer(1.0 / self._rate_hz, self._sleep_fn, + self._monotonic_fn) + + # Appended by the loop thread, read by the caller. No lock, for the + # same reason teleop's frame counter has none: list append and len are + # atomic under the GIL, and a torn sample is impossible once appended. + self._samples: List[TrajectorySample] = [] + self._thread: Optional[threading.Thread] = None + self._running = False + self._error: Optional[BaseException] = None + self._t0 = 0.0 + + # ── lifecycle ───────────────────────────────────────────────────── + + @property + def is_recording(self) -> bool: + return self._running + + @property + def sample_count(self) -> int: + return len(self._samples) + + def start(self) -> None: + """Start sampling. Raises :class:`TrajectoryBusyError` if already on.""" + if self._running: + raise TrajectoryBusyError("trajectory recording is already running") + self._samples = [] + self._error = None + self._pacer.loop_hz = 0.0 + # Sample 0 is taken here, before the thread exists, with t = 0.0: the + # recording then starts at a commanded instant instead of whenever the + # scheduler happened to first run the loop. + self._t0 = self._monotonic_fn() + self._samples.append(self._sample_now(0.0)) + self._running = True + self._thread = threading.Thread( + target=self._run, name="litegrip-trajectory-record", daemon=True) + self._thread.start() + log.info("recording started: %.0fHz zero_gravity=%s max_samples=%s", + self._rate_hz, self._zero_gravity, self._max_samples) + + def _run(self) -> None: + try: + self._loop() + except BaseException as e: # noqa: BLE001 — surfaces through result() + self._error = e + log.warning("recording loop stopped: %s", e) + finally: + self._running = False + + def _loop(self) -> None: + stall = 0 + last = self._t0 + while self._running: + t0 = self._monotonic_fn() + # A cycle whose clock did not move produced no sample: two rows with + # the same timestamp are not two measurements, and appending one + # anyway is how a stopped clock becomes a "successful" recording of + # zero length. So the clock has to move before anything is stored — + # and a clock that will not move is an error, not a slow capture. + if t0 - last <= _CLOCK_EPS: + stall += 1 + if stall >= _STALL_CYCLES: + raise TrajectoryRecordingError( + f"采样时钟连续 {stall} 拍没有前进 (t={t0}) —— " + f"拒绝在停住的时钟上无限追加采样") + last = t0 + self._pacer.rest(t0) + continue + stall = 0 + last = t0 + + # Checked before storing, not after: start() already stored the + # reference sample, so a check after the append would return + # max_samples + 1 rows — and for a cap the reference sample alone + # already meets, it would depend on which thread got there first. + if (self._max_samples is not None + and len(self._samples) >= self._max_samples): + return + + if self._zero_gravity: + self._g.send_mit_frame(q=0.0, kp=0.0, kd=0.0) + self._samples.append(self._sample_now(t0 - self._t0)) + self._pacer.rest(t0) + + def _sample_now(self, t: float) -> TrajectorySample: + state = self._g.get_state(wait=False) + return TrajectorySample( + t=float(t), + openness=rad_to_openness(state.position_rad, self._g.config), + position_rad=float(state.position_rad), + velocity_rad_s=float(state.velocity_rad_s), + torque_nm=float(state.torque_nm), + ) + + def wait_for(self, n_samples: int, timeout: float, + poll: float = 0.005) -> int: + """Block until *n_samples* have been captured; return the count. + + Polls the real clock, not the loop's ``monotonic_fn`` seam, so a test + that stubs the loop's clock still gets a bounded wait. A loop that died + raises its own error here immediately rather than after the timeout. + + Raises: + TrajectoryRecordingError: The loop died, or *timeout* elapsed with + fewer samples — the message names the count, so a short capture + is visible rather than silently accepted. + """ + n = int(n_samples) + deadline = time.monotonic() + float(timeout) + while True: + if self._error is not None: + raise self._recording_error(len(self._samples)) + got = len(self._samples) + if got >= n: + return got + if not self._running: + raise TrajectoryRecordingError( + f"录制提前结束: 只录到 {got}/{n} 拍, 循环已停止") + if time.monotonic() >= deadline: + raise TrajectoryRecordingError( + f"录制未在 {float(timeout):.1f}s 内录满 {n} 拍 " + f"(只录到 {got} 拍) —— 可调大 duration_s 或调小 rate_hz") + time.sleep(poll) + + def stop(self, timeout: float = 2.0) -> None: + """Stop the loop and leave the gripper holding its position. + + The hold happens even when the loop died on a CAN error: a failed + recording must not leave the jaws slack. + """ + self._running = False + thread = self._thread + if thread is not None and thread.is_alive(): + thread.join(timeout=timeout) + self._thread = None + if self._zero_gravity: + try: + self._g.exit_zero_gravity() + except Exception as e: # noqa: BLE001 + log.debug("exit_zero_gravity on stop failed: %s", e) + log.info("recording stopped: %d samples", len(self._samples)) + + # ── results ─────────────────────────────────────────────────────── + + def _recording_error(self, got: int) -> TrajectoryRecordingError: + """Wrap whatever killed the loop, without double-wrapping our own error.""" + if isinstance(self._error, TrajectoryRecordingError): + return self._error + return TrajectoryRecordingError( + f"录制失败, 已录到的 {got} 拍不予返回: {self._error}") + + def result(self, allow_empty: bool = False) -> Trajectory: + """The captured trajectory. + + Refuses to hand back a capture that did not work: a dead loop raises + :class:`TrajectoryRecordingError` (carrying the original error and the + sample count) rather than returning the partial capture as if it were a + whole recording. + + :class:`TrajectoryEmptyError` is for the one way to get nothing at all — + :meth:`result` on a recorder that was never started. A started capture + always holds at least the reference sample :meth:`start` takes. + + Raises: + TrajectoryRecordingError: The sampling loop died. + TrajectoryEmptyError: Nothing was ever captured. + """ + if self._error is not None: + raise self._recording_error(len(self._samples)) + if not self._samples and not allow_empty: + raise TrajectoryEmptyError( + "录制没有采到任何样本 (0 拍) —— 起停之间没有留出采样时间") + cfg = self._g.config + return Trajectory( + samples=list(self._samples), + sample_hz=self._rate_hz, + created=time.time(), + can_id=int(self._g.can_id), + pos_closed_rad=float(cfg.pos_closed_rad), + pos_open_rad=float(cfg.pos_open_rad), + rad_to_mm=float(cfg.rad_to_mm), + mount=cfg.mount, + ) + + def status(self) -> dict: + """A snapshot of the recording session, for logging and diagnostics.""" + return { + "active": self._running, + "kind": "record", + "samples": len(self._samples), + "rate_hz": round(self._rate_hz, 1), + "zero_gravity": self._zero_gravity, + "loop_hz": round(self._pacer.loop_hz, 1), + "error": None if self._error is None else str(self._error), + } + + +# ── replay ──────────────────────────────────────────────────────────────── + + +class TrajectoryPlayer: + """Streams a :class:`Trajectory` back to the gripper, on a thread. + + The target at each cycle is interpolated from the trajectory by **wall + clock**: ``u = (now - t0) * speed``, then ``trajectory.openness_at(u)``. + Advancing an index once per cycle instead would tie playback speed to the + loop rate and accumulate drift, so a cycle that overruns would make every + later sample late and a 2 s recording would take longer and longer to play. + Here a slow cycle skips ahead, and the motion stays the length it was + taught. + + The commanded angle is ``openness_to_rad(openness, local_config)``, so the + trajectory plays on a gripper with a different mount or calibration. Only + position is commanded: the recorded velocity and torque are never sent as + ``dq``/``tau`` feed-forward, because both are tied to the sign convention of + the unit that recorded them. + + Args: + gripper: The ``LiteGrip`` to drive. + trajectory: What to play. Rejected up front if it has no samples. + speed: Multiplier on the recorded timing. ``0.5`` is half speed. + kp, kd: Gains for the replay frames. ``None`` uses the gripper's + configured gains. + loop: Restart from the beginning instead of stopping at the end. A + trajectory with a single sample is a pose with no length to + restart, so looping it keeps holding that opening. + align: Move to the trajectory's first opening (one ``goto_rad``) before + following. Without it the first frame steps to sample 0 from + wherever the jaws happen to be, which is a torque spike. + rate_hz: Frame rate. ``None`` uses + :attr:`~litegrip.MotionConfig.frame_interval` (200 Hz by default). + sleep_fn, monotonic_fn: Timing seams for tests. + """ + + def __init__( + self, + gripper: Any, + trajectory: Trajectory, + speed: float = 1.0, + kp: Optional[float] = None, + kd: Optional[float] = None, + loop: bool = False, + align: bool = True, + rate_hz: Optional[float] = None, + sleep_fn: Optional[Callable[[float], None]] = None, + monotonic_fn: Optional[Callable[[], float]] = None, + ) -> None: + if speed <= 0.0: + raise ValueError(f"speed must be > 0, got {speed!r}") + if len(trajectory) == 0: + raise TrajectoryEmptyError( + "轨迹里没有任何采样点, 没有可回放的动作") + self._g = gripper + self._traj = trajectory + self._speed = float(speed) + self._kp = float(kp) if kp is not None else float(gripper.config.kp) + self._kd = float(kd) if kd is not None else float(gripper.config.kd) + self._loop = bool(loop) + self._align = bool(align) + # Where this trajectory's own clock starts. Zero for anything this + # SDK recorded; a hand-built or foreign one may not be, and + # openness_at() indexes by the absolute stamp, so the phase is kept + # separately from the elapsed time the pacing works in. + self._origin = float(trajectory.samples[0].t) + if rate_hz is None: + interval = float(gripper.motion_config.frame_interval) + rate_hz = 1.0 / interval if interval > 0.0 else 200.0 + self._rate_hz = float(rate_hz) + self._sleep_fn, self._monotonic_fn = _resolve_seams( + gripper, sleep_fn, monotonic_fn) + self._pacer = _Pacer(1.0 / self._rate_hz, self._sleep_fn, + self._monotonic_fn) + + self._thread: Optional[threading.Thread] = None + self._running = False + self._error: Optional[BaseException] = None + self._frames = 0 + self._last_openness = trajectory.samples[0].openness + self._completed = False + + # ── lifecycle ───────────────────────────────────────────────────── + + @property + def is_playing(self) -> bool: + return self._running + + @property + def is_finished(self) -> bool: + """True once the trajectory has played through to its end.""" + return self._completed + + def start(self) -> None: + """Start replaying. Raises :class:`TrajectoryBusyError` if already on.""" + if self._running: + raise TrajectoryBusyError("trajectory replay is already running") + self._running = True + self._thread = threading.Thread( + target=self._run, name="litegrip-trajectory-play", daemon=True) + self._thread.start() + log.info("replay started: %d samples, %.2fs, speed=%.2f loop=%s", + len(self._traj), self._traj.duration, self._speed, self._loop) + + def _run(self) -> None: + try: + if self._align: + self._align_to_start() + self._loop_frames() + except BaseException as e: # noqa: BLE001 — surfaces through status() + self._error = e + log.warning("replay loop stopped: %s", e) + finally: + self._running = False + + def _align_to_start(self) -> None: + """One move to sample 0, so following starts from the right place.""" + target = openness_to_rad(self._traj.samples[0].openness, self._g.config) + log.info("replay aligning to first sample: openness=%.3f -> %.3f rad", + self._traj.samples[0].openness, target) + try: + self._g.goto_rad(target, kp=self._kp, kd=self._kd, duration=1.0) + except Exception as e: # noqa: BLE001 + log.warning("replay align goto_rad failed: %s", e) + + def _loop_frames(self) -> None: + duration = self._traj.duration + t0 = self._monotonic_fn() + last = t0 + stall = 0 + while self._running: + cycle_start = self._monotonic_fn() + # The recorder's rule, for the same reason: trajectory time is + # read off this clock, so a clock that will not move means the + # replay can never reach its end — and a loop that keeps sending + # frames it cannot advance past is flooding the bus, not playing. + if cycle_start - last <= _CLOCK_EPS: + stall += 1 + if stall >= _STALL_CYCLES: + raise TrajectoryError( + f"回放时钟连续 {stall} 拍没有前进 (t={cycle_start}) —— " + f"回放推不动, 拒绝空转刷帧") + else: + stall = 0 + last = cycle_start + t0 = self._emit_cycle(cycle_start, t0, duration) + if t0 is None: + return + self._pacer.rest(cycle_start) + + def _emit_cycle(self, now: float, t0: float, + duration: float) -> Optional[float]: + """Emit one frame; return the (possibly rewound) epoch, or None to stop.""" + if duration <= 0.0: + # A one-sample trajectory is a pose, not a path: there is no time + # to advance along. Looping it means holding that pose, which is + # the only reading that keeps `loop=True` meaning "keeps going + # until play_stop" — a pose is exactly what a hold is for. + if self._loop: + self._emit(self._traj.samples[0].openness) + return t0 + self._emit(self._traj.samples[-1].openness) + self._completed = True + log.info("replay finished: %d frames", self._frames) + return None + + # Elapsed seconds along the trajectory, from the wall clock — never an + # index stepped once per cycle, which would tie the speed to the loop + # rate and make a recorded 2 s path take longer every time. + elapsed = (now - t0) * self._speed + if elapsed >= duration: + if self._loop: + # Rewind by whole loops rather than resetting to `now`: a cycle + # that overran keeps its phase instead of shifting the loop. + elapsed %= duration + t0 = now - elapsed / self._speed + else: + self._emit(self._traj.samples[-1].openness) + self._completed = True + log.info("replay finished: %d frames", self._frames) + return None + self._emit(self._traj.openness_at(self._origin + elapsed)) + return t0 + + def _emit(self, openness: float) -> None: + q = openness_to_rad(openness, self._g.config) + if not self._g.send_mit_frame(q=q, kp=self._kp, kd=self._kd, dq=0.0): + raise TrajectoryError( + "MIT 帧下发失败 (未连接或未使能?) —— 回放中止, 不静默丢帧") + self._last_openness = openness + self._frames += 1 + + def stop(self, timeout: float = 2.0) -> None: + """Stop replaying and leave the gripper holding its last target. + + One MIT frame is sent at the current position under the configured + gains. That holds the jaws for as long as frames keep arriving — the + motor self-locks a comm-loss fault about 100 ms after they stop — so + call the next action promptly, or keep the player running with + ``loop=True`` if the hold has to last. + """ + self._running = False + thread = self._thread + if thread is not None and thread.is_alive(): + thread.join(timeout=timeout) + self._thread = None + try: + self._g._hold_position() + except Exception as e: # noqa: BLE001 + log.debug("hold on replay stop failed: %s", e) + log.info("replay stopped: %d frames", self._frames) + + def wait(self, timeout: float) -> bool: + """Block until the replay finishes. True if it did, False on timeout. + + A wall-clock playback of a stalling clock would otherwise spin forever, + so the blocking wrapper gives it a deadline and this is how it asks. + """ + thread = self._thread + if thread is None: + return True + thread.join(timeout) + return not thread.is_alive() + + def status(self) -> dict: + """A snapshot of the replay session, for logging and diagnostics.""" + return { + "active": self._running, + "kind": "play", + "samples": len(self._traj), + "frames": self._frames, + "speed": round(self._speed, 3), + "loop": self._loop, + "completed": self._completed, + "openness": round(self._last_openness, 4), + "loop_hz": round(self._pacer.loop_hz, 1), + "error": None if self._error is None else str(self._error), + } diff --git a/tests/test_trajectory.py b/tests/test_trajectory.py new file mode 100644 index 0000000..26f9fc6 --- /dev/null +++ b/tests/test_trajectory.py @@ -0,0 +1,969 @@ +"""Hardware-free tests for gripper trajectory record and replay. + +The loops run for real — a thread each — against the kinematic fake CAN in +``tests/fake_can.py``. Time comes from the module's timing seams, so two clocks +appear here: + +* :func:`_use_clock` — a fake clock that advances only when the loop sleeps. + One cycle is then exactly one step of trajectory time, so a playback finishes + in microseconds and lands on the same samples every run. This is what makes + "how much path did it cover" an exact assertion instead of a timing guess. +* :func:`_pace` — the opposite: the loop parks in a short real sleep, so a + session is still running when the test looks at it. Anything that asserts + "it is running *now*" needs this one; with the fast clock the loop is often + finished before the next line executes. + +The waiting side (``wait_for``, :func:`_wait_until`) always uses the real clock, +so a loop that never advances fails a test instead of hanging it. + +The zero-gravity recorder is the one place a loop sends frames of its own, so +several tests read them back out of ``fake.frames``. That is how the +must-send-a-zero-torque-frame-every-cycle rule and the hold-on-stop rule are +checked rather than asserted in a comment. +""" + +from __future__ import annotations + +import os +import struct +import tempfile +import threading +import time +import unittest + +import _sdkpath # noqa: F401 +from litegrip import (InProcTeleopTransport, NotInitializedError, TeleopBusyError, + Trajectory, TrajectoryBusyError, TrajectoryEmptyError, + TrajectoryError, TrajectoryFormatError, + TrajectoryNotActiveError, TrajectoryPlayer, + TrajectoryRecorder, TrajectoryRecordingError, + TrajectorySample, resolve_path, trajectory_dir) +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 + +# Far longer than a stubbed loop needs. A timeout here means the loop is not +# running at all, not that the machine is slow. +WAIT_S = 2.0 + + +def _wait_until(predicate, timeout_s: float = WAIT_S) -> bool: + """Poll on the real clock until *predicate* holds, or give up.""" + deadline = time.monotonic() + timeout_s + while time.monotonic() < deadline: + if predicate(): + return True + time.sleep(0.001) + return predicate() + + +def _use_clock(g, step: float = 0.005): + """Point the loop's timing seams at a clock that moves only when it sleeps. + + Deterministic: a cycle that sleeps once advances the trajectory by exactly + *step*, whatever the loop's own frame interval is. A *step* larger than + that interval simulates a cycle which overran. + """ + now = [0.0] + + def monotonic() -> float: + return now[0] + + def sleep(_seconds: float) -> None: + now[0] += step + + g.motion_config.monotonic_fn = monotonic + g.motion_config.sleep_fn = sleep + return monotonic, sleep + + +def _frozen_clock(g) -> None: + """A clock that never moves and a sleep that costs nothing.""" + g.motion_config.monotonic_fn = lambda: 5.0 + g.motion_config.sleep_fn = lambda _seconds: None + + +def _pace(g, step: float = 0.01, real_sleep: float = 0.002) -> None: + """A clock that advances by *step* per cycle, paced by a real *real_sleep*. + + The loop keeps running — and keeps sampling — for as long as the test needs + to observe it, without the fast clock's race and without a hot spin. + """ + now = [0.0] + + def monotonic() -> float: + return now[0] + + def sleep(_seconds: float) -> None: + now[0] += step + time.sleep(real_sleep) + + g.motion_config.monotonic_fn = monotonic + g.motion_config.sleep_fn = sleep + + +def _trajectory(opennesses, dt: float = 0.01, mount: str = "normal", + reverse: bool = False, **kwargs) -> Trajectory: + """A trajectory from a list of openness values, one every *dt* seconds.""" + kwargs.setdefault("sample_hz", 1.0 / dt) + kwargs.setdefault("mount", mount) + kwargs.setdefault("rad_to_mm", RAD_TO_MM) + if reverse: + kwargs.setdefault("pos_closed_rad", POS_OPEN_RAD) + kwargs.setdefault("pos_open_rad", POS_CLOSED_RAD) + else: + kwargs.setdefault("pos_closed_rad", POS_CLOSED_RAD) + kwargs.setdefault("pos_open_rad", POS_OPEN_RAD) + return Trajectory( + samples=[TrajectorySample(t=i * dt, openness=float(o), position_rad=0.0) + for i, o in enumerate(opennesses)], + **kwargs) + + +def _commanded(fake, g): + """The openness each replayed frame asked for, in order. + + 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 — 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] + + +# ═══════════════════════════════════════════════════════════════════════════ +# File format +# ═══════════════════════════════════════════════════════════════════════════ + + +class FormatTest(unittest.TestCase): + def test_layout_is_fixed(self): + """Header and sample sizes *are* the format; changing them breaks files.""" + self.assertEqual(struct.calcsize("<8sHI5dI8s"), 66) + self.assertEqual(struct.calcsize("<5d"), 40) + self.assertEqual(len(_trajectory([0.0, 1.0]).to_bytes()), 66 + 2 * 40) + + def test_round_trip_preserves_everything(self): + original = _trajectory([0.0, 0.25, 1.0], mount="reverse", reverse=True) + original.samples[1] = TrajectorySample( + t=0.01, openness=0.25, position_rad=-0.4, velocity_rad_s=1.5, + torque_nm=-0.75) + loaded = Trajectory.from_bytes(original.to_bytes()) + self.assertEqual(loaded.samples, original.samples) + self.assertEqual(loaded.mount, "reverse") + self.assertEqual(loaded.can_id, original.can_id) + self.assertAlmostEqual(loaded.sample_hz, original.sample_hz) + self.assertAlmostEqual(loaded.rad_to_mm, RAD_TO_MM) + self.assertAlmostEqual(loaded.pos_closed_rad, POS_OPEN_RAD) + self.assertAlmostEqual(loaded.pos_open_rad, POS_CLOSED_RAD) + self.assertAlmostEqual(loaded.created, original.created) + + def test_mount_survives_both_mountings_and_none(self): + for mount in ("normal", "reverse", None): + with self.subTest(mount=mount): + traj = _trajectory([0.5], mount=mount) + self.assertEqual( + Trajectory.from_bytes(traj.to_bytes()).mount, mount) + + def _pack(self, **overrides) -> bytes: + """A valid two-sample blob, with individual header fields overridden.""" + template = _trajectory([0.0, 1.0], mount="normal") + values = dict( + magic=b"LGRTRJ01", version=1, n=2, sample_hz=100.0, + created=template.created, pos_closed_rad=POS_CLOSED_RAD, + pos_open_rad=POS_OPEN_RAD, rad_to_mm=RAD_TO_MM, can_id=0x08, + mount=b"normal\x00\x00") + values.update(overrides) + header = struct.pack( + "<8sHI5dI8s", values["magic"], values["version"], values["n"], + values["sample_hz"], values["created"], values["pos_closed_rad"], + values["pos_open_rad"], values["rad_to_mm"], values["can_id"], + values["mount"]) + body = b"".join(struct.pack("<5d", i * 0.01, float(i), 0.0, 0.0, 0.0) + for i in range(values["n"])) + return header + body + + def test_rejects_bad_magic(self): + with self.assertRaises(TrajectoryFormatError) as caught: + Trajectory.from_bytes(self._pack(magic=b"NOPE\x00\x00\x00\x00")) + self.assertIn("magic", str(caught.exception)) + + def test_rejects_unknown_version(self): + with self.assertRaises(TrajectoryFormatError) as caught: + Trajectory.from_bytes(self._pack(version=99)) + self.assertIn("99", str(caught.exception)) + + def test_rejects_truncated_payload(self): + """Length must match the header's count exactly, not approximately.""" + with self.assertRaises(TrajectoryFormatError) as caught: + Trajectory.from_bytes(self._pack()[:-3]) + self.assertIn("截断", str(caught.exception)) + + def test_rejects_trailing_bytes(self): + """Another file's tail appended is as wrong as a truncation.""" + with self.assertRaises(TrajectoryFormatError): + Trajectory.from_bytes(self._pack() + b"\x00" * 40) + + def test_rejects_a_header_that_is_too_short_to_be_one(self): + with self.assertRaises(TrajectoryFormatError): + Trajectory.from_bytes(b"LGRTRJ01\x01\x00\x02\x00") + + def test_rejects_bad_header_fields(self): + for field, value in (("sample_hz", 0.0), ("sample_hz", -5.0), + ("rad_to_mm", 0.0), + ("pos_closed_rad", POS_OPEN_RAD), + ("mount", b"sideways")): + with self.subTest(field=field, value=value): + with self.assertRaises(TrajectoryFormatError): + Trajectory.from_bytes(self._pack(**{field: value})) + + def test_rejects_corrupt_samples(self): + header = self._pack()[:66] + cases = { + "nan": struct.pack("<5d", 0.0, float("nan"), 0.0, 0.0, 0.0), + "inf": struct.pack("<5d", 0.0, 0.5, float("inf"), 0.0, 0.0), + "openness above 1": struct.pack("<5d", 0.0, 1.5, 0.0, 0.0, 0.0), + "openness below 0": struct.pack("<5d", 0.0, -0.1, 0.0, 0.0, 0.0), + } + for label, first in cases.items(): + with self.subTest(case=label): + second = struct.pack("<5d", 0.01, 0.5, 0.0, 0.0, 0.0) + with self.assertRaises(TrajectoryFormatError): + Trajectory.from_bytes(header + first + second) + + def test_rejects_time_going_backwards(self): + header = self._pack()[:66] + body = (struct.pack("<5d", 0.5, 0.0, 0.0, 0.0, 0.0) + + struct.pack("<5d", 0.1, 1.0, 0.0, 0.0, 0.0)) + with self.assertRaises(TrajectoryFormatError) as caught: + Trajectory.from_bytes(header + body) + self.assertIn("单调", str(caught.exception)) + + +class InterpolationTest(unittest.TestCase): + def test_interpolates_between_samples(self): + traj = _trajectory([0.0, 1.0], dt=1.0) + self.assertAlmostEqual(traj.openness_at(0.25), 0.25) + self.assertAlmostEqual(traj.openness_at(0.75), 0.75) + self.assertAlmostEqual(traj.duration, 1.0) + self.assertEqual(len(traj), 2) + + def test_clamps_outside_the_recorded_span(self): + traj = _trajectory([0.0, 1.0], dt=1.0) + self.assertAlmostEqual(traj.openness_at(-3.0), 0.0) + self.assertAlmostEqual(traj.openness_at(9.0), 1.0) + + def test_interpolates_across_uneven_sampling(self): + traj = Trajectory(samples=[ + TrajectorySample(t=0.0, openness=0.0, position_rad=0.0), + TrajectorySample(t=0.1, openness=0.2, position_rad=0.0), + TrajectorySample(t=0.4, openness=1.0, position_rad=0.0), + ]) + self.assertAlmostEqual(traj.openness_at(0.05), 0.1) + self.assertAlmostEqual(traj.openness_at(0.25), 0.6) + self.assertAlmostEqual(traj.openness_at(0.1), 0.2) + + def test_a_single_sample_has_no_duration(self): + self.assertEqual(Trajectory().duration, 0.0) + self.assertEqual(len(Trajectory()), 0) + self.assertEqual(_trajectory([0.7]).duration, 0.0) + + def test_duration_is_the_span_not_the_last_timestamp(self): + """A trajectory shifted off zero lasts as long as its own samples cover.""" + traj = Trajectory(samples=[ + TrajectorySample(t=5.0, openness=0.0, position_rad=0.0), + TrajectorySample(t=6.0, openness=1.0, position_rad=0.0), + ]) + self.assertAlmostEqual(traj.duration, 1.0) + + def test_openness_at_on_an_empty_trajectory_raises(self): + with self.assertRaises(TrajectoryEmptyError): + Trajectory().openness_at(0.0) + + +class PathTest(unittest.TestCase): + """The name-to-path rule and the file it produces.""" + + def setUp(self): + self._env = os.environ.get("LITEGRIP_TRAJ_DIR") + self._home = os.environ.get("HOME") + self._root = tempfile.mkdtemp(prefix="litegrip-traj-") + self.addCleanup(self._restore) + + def _restore(self): + for key, value in (("LITEGRIP_TRAJ_DIR", self._env), + ("HOME", self._home)): + if value is None: + os.environ.pop(key, None) + else: + os.environ[key] = value + + def test_default_directory_is_next_to_the_calibrations(self): + os.environ.pop("LITEGRIP_TRAJ_DIR", None) + os.environ["HOME"] = "/home/someone" + self.assertEqual(trajectory_dir(), "/home/someone/.litegrip/trajectories") + + def test_env_override_wins(self): + os.environ["LITEGRIP_TRAJ_DIR"] = "/tmp/trajectories" + self.assertEqual(trajectory_dir(), "/tmp/trajectories") + + def test_a_bare_name_lands_in_the_directory_with_an_extension(self): + os.environ["LITEGRIP_TRAJ_DIR"] = "/tmp/lg" + self.assertEqual(resolve_path("pick"), "/tmp/lg/pick.lgt") + self.assertEqual(resolve_path("pick.lgt"), "/tmp/lg/pick.lgt") + + def test_a_path_is_used_as_written(self): + self.assertEqual(resolve_path("out/pick.lgt"), "out/pick.lgt") + self.assertEqual(resolve_path("/tmp/pick.lgt"), "/tmp/pick.lgt") + + def test_save_creates_the_directory_and_load_reads_it_back(self): + os.environ["LITEGRIP_TRAJ_DIR"] = os.path.join(self._root, "nested") + traj = _trajectory([0.0, 0.5, 1.0], mount="reverse", reverse=True) + written = traj.save("demo") + self.assertEqual(written, + os.path.join(self._root, "nested", "demo.lgt")) + self.assertTrue(os.path.isfile(written)) + self.assertEqual(Trajectory.load("demo").samples, traj.samples) + self.assertEqual(Trajectory.load(written).mount, "reverse") + + def test_load_rejects_a_file_that_is_not_a_trajectory(self): + os.environ["LITEGRIP_TRAJ_DIR"] = self._root + with open(resolve_path("junk"), "wb") as f: + f.write(b"definitely not a trajectory") + with self.assertRaises(TrajectoryFormatError): + Trajectory.load("junk") + + +# ═══════════════════════════════════════════════════════════════════════════ +# Recording +# ═══════════════════════════════════════════════════════════════════════════ + + +class RecorderTest(unittest.TestCase): + def test_record_captures_the_timed_number_of_samples(self): + g, _ = make_gripper() + _use_clock(g) + traj = g.record(0.5, rate_hz=100.0) + self.assertEqual(len(traj), 50) + self.assertEqual(traj.sample_hz, 100.0) + + def test_the_reference_sample_is_taken_at_t_zero_before_any_frame(self): + """Sample 0 is taken by start(), so the count and t=0 are not raced.""" + g, fake = make_gripper() + _use_clock(g) + before = fake.motor.reported_pos() + traj = g.record(0.05, rate_hz=100.0) + first = traj.samples[0] + self.assertEqual(first.t, 0.0) + self.assertEqual(first.position_rad, before) + self.assertAlmostEqual(first.openness, rad_to_openness(before, g.config)) + + def test_a_recording_carries_the_calibration_it_was_taken_against(self): + g, _ = make_gripper(reverse=True) + _use_clock(g) + traj = g.record(0.03, rate_hz=100.0) + self.assertEqual(traj.mount, "reverse") + self.assertAlmostEqual(traj.pos_closed_rad, POS_OPEN_RAD) + self.assertAlmostEqual(traj.pos_open_rad, POS_CLOSED_RAD) + self.assertAlmostEqual(traj.rad_to_mm, RAD_TO_MM) + self.assertEqual(traj.can_id, g.can_id) + + def test_record_requires_a_positive_duration(self): + g, _ = make_gripper() + for bad in (0.0, -1.0): + with self.subTest(duration_s=bad): + with self.assertRaises(ValueError): + g.record(bad) + + def test_record_requires_calibration(self): + """Without a real calibration the normalised opening would be a guess.""" + g, _ = make_gripper() + g.config.calibrated = False + with self.assertRaises(TrajectoryError) as caught: + g.record(0.1) + self.assertIn("标定", str(caught.exception)) + self.assertIsNone(g.session, "a refused start must not claim the session") + + def test_record_requires_connection_and_enable(self): + g, _ = make_gripper() + g._connected = False + with self.assertRaises(NotInitializedError): + g.record(0.1) + g._connected = True + g._enabled = False + with self.assertRaises(NotInitializedError): + g.record(0.1) + self.assertIsNone(g.session) + + def test_start_stop_round_trip_reaches_the_cap(self): + g, _ = make_gripper() + _pace(g) + status = g.record_start(rate_hz=100.0, max_samples=10) + self.assertTrue(status["active"]) + self.assertEqual(status["kind"], "record") + self.assertTrue(_wait_until(lambda: not g.trajectory_status()["active"]), + "the recorder should stop itself at max_samples") + traj = g.record_stop() + self.assertEqual(len(traj), 10) + self.assertIsNone(g.session) + + def test_the_cap_counts_the_reference_sample(self): + """`max_samples=1` is one sample, not the reference plus one more.""" + g, _ = make_gripper() + _pace(g) + g.record_start(rate_hz=100.0, max_samples=1) + self.assertTrue(_wait_until(lambda: not g.trajectory_status()["active"])) + self.assertEqual(len(g.record_stop()), 1) + + def test_a_capture_shorter_than_one_sample_interval_yields_one(self): + """record() rounds the target up to one sample, and stops there.""" + g, _ = make_gripper() + _use_clock(g) + self.assertEqual(len(g.record(0.004, rate_hz=100.0)), 1) + + def test_stopping_a_recording_that_is_not_running_is_an_error(self): + g, _ = make_gripper() + with self.assertRaises(TrajectoryNotActiveError): + g.record_stop() + + def test_a_second_start_while_recording_is_busy(self): + g, _ = make_gripper() + _pace(g) + g.record_start(rate_hz=100.0) + try: + with self.assertRaises(TrajectoryBusyError) as caught: + g.record_start(rate_hz=100.0) + self.assertIn("record", str(caught.exception)) + finally: + g.record_stop(allow_empty=True) + + def test_a_dead_loop_is_not_returned_as_a_capture(self): + """A CAN error mid-recording must not come back as a short trajectory.""" + g, fake = make_gripper() + _use_clock(g) + original = fake.control_mit + calls = [] + + def failing(*args, **kwargs): + calls.append(1) + if len(calls) > 3: + raise RuntimeError("bus down") + return original(*args, **kwargs) + + fake.control_mit = failing + g.record_start(rate_hz=100.0, max_samples=1000) + self.assertTrue(_wait_until(lambda: not g.trajectory_status()["active"])) + with self.assertRaises(TrajectoryRecordingError) as caught: + g.record_stop() + self.assertIn("bus down", str(caught.exception)) + self.assertGreater(len(calls), 3) + self.assertIsNone(g.session, "the session must be released on failure") + + def test_a_timed_capture_that_did_not_fill_raises(self): + """record() never hands back a short capture as if it were whole.""" + g, _ = make_gripper() + _frozen_clock(g) + with self.assertRaises(TrajectoryRecordingError) as caught: + g.record(0.1, rate_hz=100.0) + self.assertIn("没有前进", str(caught.exception)) + self.assertIsNone(g.session) + self.assertIsNone(g.trajectory_status()["kind"]) + + def test_a_clock_that_never_advances_cannot_spin_forever(self): + """The non-advancing-cursor rule, ported to a sampling clock.""" + g, _ = make_gripper() + _frozen_clock(g) + recorder = TrajectoryRecorder(g, rate_hz=1000.0, max_samples=100000) + recorder.start() + try: + with self.assertRaises(TrajectoryRecordingError) as caught: + recorder.wait_for(100000, timeout=5.0) + self.assertIn("没有前进", str(caught.exception)) + self.assertEqual(recorder.sample_count, 1, + "only the reference sample exists; a repeated " + "timestamp is not a second measurement") + finally: + recorder.stop() + + def test_result_on_a_recorder_that_never_started_is_empty(self): + g, _ = make_gripper() + recorder = TrajectoryRecorder(g) + with self.assertRaises(TrajectoryEmptyError): + recorder.result() + self.assertEqual(len(recorder.result(allow_empty=True)), 0) + + def test_zero_gravity_streams_a_torque_free_frame_every_cycle(self): + """The motor faults ~100 ms after frames stop, so recording must send.""" + g, fake = make_gripper() + _use_clock(g) + traj = g.record(0.05, rate_hz=100.0, zero_gravity=True) + streamed = fake.frames[:-1] # the last one is the hold on stop + self.assertEqual(len(streamed), len(traj) - 1, + "every sample but the reference one has its frame") + for frame in streamed: + self.assertEqual((frame.kp, frame.kd), (0.0, 0.0)) + self.assertEqual(frame.q, 0.0) + + def test_a_read_only_recording_never_touches_the_bus(self): + """With zero_gravity off the caller drives, so the recorder only reads.""" + g, fake = make_gripper() + _use_clock(g) + g.record(0.05, rate_hz=100.0, zero_gravity=False) + self.assertEqual(fake.frames, []) + + def test_record_stop_leaves_the_gripper_holding_not_slack(self): + g, fake = make_gripper() + _use_clock(g) + g.record(0.05, rate_hz=100.0, zero_gravity=True) + last = fake.frames[-1] + self.assertAlmostEqual(last.kp, g.config.kp) + self.assertAlmostEqual(last.kd, g.config.kd) + self.assertEqual(last.q, fake.motor.reported_pos()) + + def test_status_shape_while_recording(self): + g, _ = make_gripper() + _pace(g) + status = g.record_start(rate_hz=50.0) + try: + self.assertEqual(status["kind"], "record") + self.assertTrue(status["active"]) + self.assertEqual(status["rate_hz"], 50.0) + self.assertTrue(status["zero_gravity"]) + self.assertIsNone(status["error"]) + self.assertIn("samples", status) + finally: + g.record_stop(allow_empty=True) + + +# ═══════════════════════════════════════════════════════════════════════════ +# Replay +# ═══════════════════════════════════════════════════════════════════════════ + + +class PlayerTest(unittest.TestCase): + def test_play_requires_calibration(self): + g, _ = make_gripper() + g.config.calibrated = False + with self.assertRaises(TrajectoryError) as caught: + g.play(_trajectory([0.0, 1.0])) + self.assertIn("标定", str(caught.exception)) + self.assertIsNone(g.session) + + def test_play_rejects_an_empty_trajectory(self): + g, _ = make_gripper() + with self.assertRaises(TrajectoryEmptyError): + g.play(Trajectory()) + self.assertIsNone(g.session) + + def test_play_rejects_a_non_positive_speed(self): + g, _ = make_gripper() + for bad in (0.0, -1.0): + with self.subTest(speed=bad): + with self.assertRaises(ValueError): + g.play(_trajectory([0.0, 1.0]), speed=bad) + self.assertIsNone(g.session) + + def test_blocking_play_rejects_loop(self): + """A looping blocking replay could never return.""" + g, _ = make_gripper() + with self.assertRaises(ValueError) as caught: + g.play(_trajectory([0.0, 1.0]), loop=True) + self.assertIn("play_start", str(caught.exception)) + self.assertIsNone(g.session) + + def test_an_interrupted_blocking_replay_releases_the_session(self): + """Ctrl+C out of play() must stop the player, not just unwind.""" + g, fake = make_gripper() + _pace(g, step=0.05) + original = TrajectoryPlayer.wait + + def interrupted(self, timeout): + raise KeyboardInterrupt + + TrajectoryPlayer.wait = interrupted + self.addCleanup(setattr, TrajectoryPlayer, "wait", original) + with self.assertRaises(KeyboardInterrupt): + g.play(_trajectory([0.0, 1.0], dt=100.0), align=False) + self.assertIsNone(g.session, "the session must not stay claimed") + self.assertEqual(g.trajectory_status(), {"active": False, "kind": None}) + frames = len(fake.frames) + time.sleep(0.05) # several paced cycles + self.assertEqual(len(fake.frames), frames, + "the player must be stopped, not still commanding") + + def test_follows_the_trajectory_from_start_to_end(self): + g, fake = make_gripper() + _use_clock(g) + status = g.play(_trajectory([0.0, 0.5, 1.0], dt=0.5), align=False) + self.assertFalse(status["active"]) + self.assertTrue(status["completed"]) + commanded = _commanded(fake, g) + self.assertAlmostEqual(commanded[0], 0.0, places=6) + self.assertAlmostEqual(commanded[-1], 1.0, places=6) + self.assertEqual(commanded, sorted(commanded), "the path is monotone") + self.assertGreater(len(commanded), 50, "a ramp, not just the endpoints") + + def test_the_pace_is_wall_clock_not_one_index_per_cycle(self): + """Frames land between the samples: the path is interpolated.""" + g, fake = make_gripper() + _use_clock(g, step=0.125) # 8 frames over a 1 s trajectory + g.play(_trajectory([0.0, 1.0], dt=1.0), align=False) + commanded = _commanded(fake, g) + self.assertAlmostEqual(commanded[0], 0.0, places=9) + self.assertAlmostEqual(commanded[1], 0.125, places=9) + self.assertAlmostEqual(commanded[2], 0.25, places=9) + self.assertAlmostEqual(commanded[-1], 1.0, places=9) + + def test_a_nonzero_first_timestamp_does_not_delay_the_start(self): + """t is relative to the recording, so a shifted one still plays at once.""" + traj = _trajectory([0.0, 1.0], dt=0.5) + traj.samples = [TrajectorySample(t=s.t + 5.0, openness=s.openness, + position_rad=s.position_rad) + for s in traj.samples] + g, fake = make_gripper() + _use_clock(g, step=0.125) + g.play(traj, align=False) + commanded = _commanded(fake, g) + self.assertEqual(len(commanded), 5) + self.assertAlmostEqual(commanded[0], 0.0, places=9) + self.assertAlmostEqual(commanded[-1], 1.0, places=9) + + def test_aligning_moves_to_the_first_sample_before_following(self): + g, fake = make_gripper() + _use_clock(g) + g.play(_trajectory([0.25, 1.0], dt=0.5), align=True) + self.assertAlmostEqual( + rad_to_openness(fake.frames[0].q, g.config), 0.25, places=6) + + def test_speed_scales_how_long_the_replay_takes(self): + """Half speed means twice the frames for the same trajectory.""" + fast_g, fast_fake = make_gripper() + _use_clock(fast_g) + fast_g.play(_trajectory([0.0, 1.0], dt=0.5), align=False) + + slow_g, slow_fake = make_gripper() + _use_clock(slow_g) + slow_g.play(_trajectory([0.0, 1.0], dt=0.5), speed=0.5, align=False) + + self.assertAlmostEqual(len(_commanded(slow_fake, slow_g)), + 2 * len(_commanded(fast_fake, fast_g)), delta=2) + + def test_a_slow_cycle_skips_ahead_instead_of_lagging(self): + """Playback is wall-clock driven, so an overrun does not stretch time.""" + g, fake = make_gripper() + _use_clock(g, step=0.25) # each cycle covers 0.25 s of path + g.play(_trajectory([0.0, 0.5, 1.0], dt=0.5), align=False) + commanded = _commanded(fake, g) + self.assertEqual(len(commanded), 5, + "an overrunning cycle must skip, not replay 200 frames") + self.assertAlmostEqual(commanded[-1], 1.0, places=9) + + def test_the_last_frame_is_the_end_and_the_stop_holds_under_the_gains(self): + g, fake = make_gripper() + step = g.motion_config.frame_interval + _use_clock(g, step=step) + g.play_start(_trajectory([0.0, 1.0], dt=0.5), align=False) + self.assertTrue(_wait_until(lambda: not g.trajectory_status()["active"])) + g.play_stop() + # One frame per cycle across the whole span, both ends included. + self.assertEqual(len(_commanded(fake, g)), int(round(0.5 / step)) + 1) + self.assertAlmostEqual(fake.frames[-2].q, + openness_to_rad(1.0, g.config), places=9) + hold = fake.frames[-1] + self.assertAlmostEqual(hold.q, fake.motor.reported_pos()) + self.assertAlmostEqual(hold.kp, g.config.kp) + for frame in fake.frames: + self.assertNotEqual(frame.kp, 0.0, + "replay must never leave the jaws slack") + + def test_recorded_torque_and_velocity_are_not_replayed(self): + """Recorded tau/dq are diagnostics; they are sign- and mount-dependent.""" + g, fake = make_gripper() + _use_clock(g) + traj = _trajectory([0.0, 1.0], dt=0.5) + traj.samples = [TrajectorySample(t=s.t, openness=s.openness, + position_rad=-9.0, velocity_rad_s=7.5, + torque_nm=3.25) + for s in traj.samples] + g.play(traj, align=False) + for frame in fake.frames: + self.assertEqual(frame.tau_ff, 0.0) + self.assertEqual(frame.dq, 0.0) + + def test_loop_wraps_instead_of_stopping(self): + g, fake = make_gripper() + _pace(g, step=0.05) + g.play_start(_trajectory([0.0, 1.0], dt=0.1), loop=True, align=False) + try: + self.assertTrue(_wait_until(lambda: len(fake.frames) > 60), + "a looping replay should keep sending frames") + status = g.trajectory_status() + self.assertTrue(status["active"]) + self.assertFalse(status["completed"]) + finally: + g.play_stop() + self.assertIsNone(g.session) + + def test_a_failing_send_aborts_rather_than_dropping_frames(self): + g, _ = make_gripper() + _use_clock(g) + g.send_mit_frame = lambda **kwargs: False + g.play_start(_trajectory([0.0, 1.0], dt=0.1), align=False) + self.assertTrue(_wait_until(lambda: not g.trajectory_status()["active"])) + status = g.play_stop() + self.assertIsNotNone(status["error"]) + self.assertIn("MIT 帧下发失败", status["error"]) + self.assertIsNone(g.session) + + def test_looping_a_one_sample_trajectory_holds_that_pose(self): + """A pose has no length to advance along; looping it must still hold.""" + g, fake = make_gripper() + _pace(g, step=0.05) + g.play_start(_trajectory([0.7]), loop=True, align=False) + try: + self.assertTrue(_wait_until(lambda: len(fake.frames) > 20), + "a held pose must keep sending frames") + self.assertFalse(g.trajectory_status()["completed"]) + finally: + g.play_stop() + for frame in fake.frames[:-1]: # the last one is the hold on stop + self.assertAlmostEqual(rad_to_openness(frame.q, g.config), 0.7, + places=9) + + def test_a_clock_that_never_advances_cannot_flood_the_bus(self): + """The player carries the recorder's stall rule, for the same reason.""" + g, fake = make_gripper() + _frozen_clock(g) + g.play_start(_trajectory([0.0, 1.0], dt=5.0), align=False) + self.assertTrue(_wait_until(lambda: not g.trajectory_status()["active"])) + status = g.play_stop() + self.assertIn("没有前进", status["error"]) + # A few frames before the guard fires, then the hold on stop — not the + # unbounded stream a wall-clock-driven loop would otherwise emit. + self.assertLessEqual(len(fake.frames), 8) + self.assertIsNone(g.session) + + def test_play_stop_leaves_the_gripper_holding(self): + g, fake = make_gripper() + _pace(g, step=0.05) + g.play_start(_trajectory([0.0, 1.0], dt=100.0), align=False) + try: + self.assertTrue(_wait_until(lambda: len(fake.frames) > 3)) + finally: + g.play_stop() + last = fake.frames[-1] + self.assertAlmostEqual(last.kp, g.config.kp) + self.assertEqual(last.q, fake.motor.reported_pos()) + + def test_status_shape_while_replaying(self): + g, _ = make_gripper() + _pace(g, step=0.05) + status = g.play_start(_trajectory([0.0, 1.0], dt=100.0), speed=0.5, + align=False) + try: + self.assertEqual(status["kind"], "play") + self.assertTrue(status["active"]) + self.assertEqual(status["samples"], 2) + self.assertEqual(status["speed"], 0.5) + self.assertFalse(status["loop"]) + finally: + g.play_stop() + + +class PortabilityTest(unittest.TestCase): + """The point of storing openness: a trajectory replays on another unit.""" + + def _replay(self, reverse: bool): + g, fake = make_gripper(reverse=reverse) + _use_clock(g) + g.play(_trajectory([0.0, 0.4, 1.0], dt=0.5), align=False) # normal-mount + return g, fake, _commanded(fake, g) + + def test_openness_is_reproduced_on_a_reverse_mounted_unit(self): + normal_g, normal_fake, normal = self._replay(reverse=False) + reverse_g, reverse_fake, reverse = self._replay(reverse=True) + + self.assertEqual(len(normal), len(reverse)) + self.assertAlmostEqual(normal[0], reverse[0], places=6) + self.assertAlmostEqual(normal[-1], reverse[-1], places=6) + + # Same motion, opposite raw angle: the mount flips the sign, nothing else. + self.assertEqual(normal_g.config.close_sign, 1.0) + self.assertEqual(reverse_g.config.close_sign, -1.0) + self.assertLess(normal_fake.frames[-1].q, normal_fake.frames[0].q) + self.assertGreater(reverse_fake.frames[-1].q, reverse_fake.frames[0].q) + + def test_openness_endpoints_map_to_each_units_own_limits(self): + for reverse in (False, True): + g, _ = make_gripper(reverse=reverse) + with self.subTest(reverse=reverse): + self.assertAlmostEqual(openness_to_rad(0.0, g.config), + g.config.pos_closed_rad, places=9) + self.assertAlmostEqual(openness_to_rad(1.0, g.config), + g.config.pos_open_rad, places=9) + + def test_a_stored_opening_beyond_travel_is_clamped(self): + """A hand-edited or foreign file cannot command past a mechanical stop.""" + traj = Trajectory(samples=[ + TrajectorySample(t=0.0, openness=5.0, position_rad=0.0), + TrajectorySample(t=0.5, openness=-5.0, position_rad=0.0), + ]) + g, fake = make_gripper() + _use_clock(g) + g.play(traj, align=False) + limits = sorted([g.config.pos_closed_rad, g.config.pos_open_rad]) + for frame in fake.frames: + self.assertGreaterEqual(frame.q, limits[0] - 1e-9) + self.assertLessEqual(frame.q, limits[1] + 1e-9) + + +# ═══════════════════════════════════════════════════════════════════════════ +# Sessions +# ═══════════════════════════════════════════════════════════════════════════ + + +class SessionTest(unittest.TestCase): + def test_status_is_idle_when_nothing_runs(self): + g, _ = make_gripper() + self.assertEqual(g.trajectory_status(), {"active": False, "kind": None}) + self.assertIsNone(g.session) + + def test_stopping_when_idle_is_harmless(self): + g, _ = make_gripper() + self.assertEqual(g.play_stop(), {"active": False, "kind": None}) + + def test_teleop_excludes_recording_and_replay(self): + g, _ = make_gripper() + _pace(g) + g.teleop_start("master", transport=InProcTeleopTransport()) + try: + self.assertEqual(g.session, "teleop") + with self.assertRaises(TrajectoryBusyError) as caught: + g.record_start(rate_hz=100.0) + self.assertIn("teleop", str(caught.exception)) + with self.assertRaises(TrajectoryBusyError): + g.play_start(_trajectory([0.0, 1.0])) + finally: + g.teleop_stop() + self.assertIsNone(g.session) + + def test_teleop_refuses_to_start_while_recording(self): + """Teleop keeps its own error type, and names what is in the way.""" + g, _ = make_gripper() + _pace(g) + g.record_start(rate_hz=100.0) + try: + with self.assertRaises(TeleopBusyError) as caught: + g.teleop_start("master", transport=InProcTeleopTransport()) + self.assertIn("record", str(caught.exception)) + self.assertEqual(g.session, "record") + finally: + g.record_stop(allow_empty=True) + + def test_recording_and_replay_cannot_both_run(self): + g, _ = make_gripper() + _pace(g) + g.record_start(rate_hz=100.0) + try: + with self.assertRaises(TrajectoryBusyError): + g.play_start(_trajectory([0.0, 1.0])) + finally: + g.record_stop(allow_empty=True) + self.assertIsNone(g.session) + + def test_only_one_of_two_racing_starts_wins(self): + """The session is claimed under a lock, not checked and then taken.""" + g, _ = make_gripper() + _pace(g) + outcomes = [] + barrier = threading.Barrier(2) + + def start_recording(): + barrier.wait() + try: + g.record_start(rate_hz=100.0) + outcomes.append("record") + except TrajectoryBusyError: + outcomes.append("refused") + + def start_playing(): + barrier.wait() + try: + g.play_start(_trajectory([0.0, 1.0]), align=False) + outcomes.append("play") + except TrajectoryBusyError: + outcomes.append("refused") + + threads = [threading.Thread(target=start_recording), + threading.Thread(target=start_playing)] + for thread in threads: + thread.start() + for thread in threads: + thread.join(timeout=WAIT_S) + try: + self.assertEqual(len(outcomes), 2, outcomes) + self.assertEqual(outcomes.count("refused"), 1, outcomes) + finally: + if g.session == "record": + g.record_stop(allow_empty=True) + elif g.session == "play": + g.play_stop() + self.assertIsNone(g.session) + + def test_disconnect_stops_a_running_recording(self): + """The loop has to be stopped, not just unlinked from the gripper.""" + g, fake = make_gripper() + _pace(g) + g.record_start(rate_hz=100.0) + recorder = g._trajectory_recorder + self.assertTrue(recorder.is_recording) + g.disconnect() + self.assertFalse(recorder.is_recording) + # Only TrajectoryRecorder.stop() clears this, so it pins the teardown + # rather than the bookkeeping that runs either way. + self.assertIsNone(recorder._thread) + # stop() leaves zero gravity, sending one hold frame under the + # configured gains — the gripper is held, not left slack. + self.assertAlmostEqual(fake.frames[-1].kp, g.config.kp) + self.assertEqual(g.trajectory_status(), {"active": False, "kind": None}) + self.assertIsNone(g.session) + + def test_disconnect_stops_a_running_replay(self): + g, fake = make_gripper() + _pace(g, step=0.05) + g.play_start(_trajectory([0.0, 1.0], dt=100.0), align=False) + player = g._trajectory_player + self.assertTrue(_wait_until(lambda: len(fake.frames) > 3)) + g.disconnect() + self.assertFalse(player.is_playing) + self.assertIsNone(player._thread, + "disconnect must join the player, not just drop it") + self.assertIsNone(g.session) + frames = len(fake.frames) + time.sleep(0.05) # several paced cycles + self.assertEqual(len(fake.frames), frames, + "the player must not still be commanding the motor") + + +class SeamTest(unittest.TestCase): + """The loops take their timing from MotionConfig, not from ``time``.""" + + def test_explicit_seams_win_over_the_config(self): + g, _ = make_gripper() + monotonic = lambda: 0.0 # noqa: E731 + sleep = lambda _s: None # noqa: E731 + recorder = TrajectoryRecorder(g, sleep_fn=sleep, monotonic_fn=monotonic) + self.assertIs(recorder._sleep_fn, sleep) + self.assertIs(recorder._monotonic_fn, monotonic) + + def test_the_loops_fall_back_to_the_config(self): + g, _ = make_gripper() + g.motion_config.monotonic_fn = lambda: 1.0 + recorder = TrajectoryRecorder(g) + player = TrajectoryPlayer(g, _trajectory([0.0, 1.0])) + self.assertIs(recorder._monotonic_fn, g.motion_config.monotonic_fn) + self.assertIs(player._monotonic_fn, g.motion_config.monotonic_fn) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/trajectory_check.py b/tests/trajectory_check.py new file mode 100644 index 0000000..762b1e6 --- /dev/null +++ b/tests/trajectory_check.py @@ -0,0 +1,522 @@ +"""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())