From c014b6903b22b2acff893187a00c19cecb58f22c Mon Sep 17 00:00:00 2001 From: cao-xiao-hao <127832643+cao-xiao-hao@users.noreply.github.com> Date: Tue, 29 Sep 2026 14:54:10 +0800 Subject: [PATCH 1/2] feat: add gripper trajectory record and replay The SDK could move the jaws and mirror one gripper onto another, but it could not teach a motion once and repeat it: every approach path, seating wiggle or squeeze profile had to be re-written as code, with no way to capture one off the hardware. Recording puts the motor into zero-gravity so the jaws can be pushed by hand, samples the opening, and can save it to a file. Replay streams the captured openings back as MIT command frames, on a wall clock rather than an index per cycle, so a slow cycle skips ahead instead of making the motion longer than it was taught. What is stored is the opening normalised by the recording unit's travel -- the same channel teleop sends -- so a trajectory taught on a normal-mount gripper replays on a reverse-mounted one. Recorded torque and velocity are kept as diagnostics and never fed forward: replay commands position, not force, so a squeeze that was taught against an object repeats as a position path and not as the same grip force. Recording, replay and teleoperation now claim one session slot under a lock, so two of them cannot interleave frames on one motor; disconnect tears whichever is running down. The hold-position frame the teleop master and a stopping replay both need is one extracted primitive. Traps worked around, following litearm-core's logging rules: a sample whose timestamp did not advance is not stored and a stopped clock aborts either loop; a capture that did not fill raises and reports the count instead of returning a short recording as if it were whole; and a file whose length does not match the sample count in its header is rejected rather than parsed into half a trajectory. --- README.md | 66 +++ examples/trajectory.py | 193 ++++++++ readme_zn.md | 57 +++ src/litegrip/__init__.py | 31 ++ src/litegrip/gripper.py | 420 +++++++++++++++- src/litegrip/trajectory.py | 918 +++++++++++++++++++++++++++++++++++ tests/test_trajectory.py | 969 +++++++++++++++++++++++++++++++++++++ 7 files changed, 2629 insertions(+), 25 deletions(-) create mode 100644 examples/trajectory.py create mode 100644 src/litegrip/trajectory.py create mode 100644 tests/test_trajectory.py diff --git a/README.md b/README.md index c603484..1f5241c 100644 --- a/README.md +++ b/README.md @@ -197,6 +197,72 @@ the frame the slave followed — a caller can show the jaws without opening a se - Follow gains default to the calibration's `kp` / `kd` (`100.0` / `2.0` out of the box); 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 b2a2608..1931801 100644 --- a/readme_zn.md +++ b/readme_zn.md @@ -174,6 +174,63 @@ python3 examples/teleop.py --mode slave --channel can0 --host 192.168.1.20 驱动之前就拒掉。 - 跟随增益默认取标定里的 `kp` / `kd`（出厂是 `100.0` / `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 11c5c3c..d6c696a 100644 --- a/src/litegrip/__init__.py +++ b/src/litegrip/__init__.py @@ -131,6 +131,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 @@ -222,6 +239,20 @@ def __dir__(): "Listener", "Connector", "LatestSlot", + # 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 aa98930..fe3761f 100644 --- a/src/litegrip/gripper.py +++ b/src/litegrip/gripper.py @@ -253,6 +253,17 @@ def __init__( # port bound and makes matching fail intermittently. self._teleop_pub: 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 @@ -371,6 +382,17 @@ def disconnect(self) -> None: if self._teleop is not None: self.teleop_stop() self._close_teleop_pub() + # A recording left running would keep sampling a bus 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) @@ -559,10 +581,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) @@ -1846,32 +1886,44 @@ def teleop_start( check_ready(self.config) if self._teleop is not None and self._teleop.is_running: raise TeleopBusyError("teleop is already running") + self._claim_session("teleop", TeleopBusyError) - key = teleop_topic(grip_id) - created_transport = None - if transport is None: - if link == "zenoh": - if mode == "master": - transport = created_transport = self._open_teleop_pub(port, key) - else: - transport = created_transport = _zenoh_transport( - "slave", key, port, host) - elif link == "udp": - if host is None: - raise ValueError("host is required for the udp link") - addr = f"{host}:{port}" - if mode == "master": - transport = created_transport = UdpTeleopTransport(pub_addr=addr) + # Everything from here to the running loop is inside one try: the + # session was claimed above, and a transport that refuses (a bad link, + # a missing host, a port already bound) would otherwise leave the slot + # claimed for the life of the process, refusing every later teleop, + # record and replay in turn. + try: + key = teleop_topic(grip_id) + created_transport = None + if transport is None: + if link == "zenoh": + if mode == "master": + transport = self._open_teleop_pub(port, key) + else: + transport = _zenoh_transport("slave", key, port, host) + created_transport = transport + elif link == "udp": + if host is None: + raise ValueError("host is required for the udp link") + addr = f"{host}:{port}" + if mode == "master": + transport = UdpTeleopTransport(pub_addr=addr) + else: + transport = UdpTeleopTransport(bind_addr=addr) + created_transport = transport else: - transport = created_transport = UdpTeleopTransport(bind_addr=addr) - else: - raise ValueError(f"link must be 'zenoh' or 'udp', got {link!r}") - - manager = GripperTeleop( - self, transport, mode, key, - rate_hz=rate_hz, kp=kp, kd=kd, align=align, watchdog_s=watchdog_s, - dq_max=dq_max) - manager.start() + raise ValueError( + f"link must be 'zenoh' or 'udp', got {link!r}") + + manager = GripperTeleop( + self, transport, mode, key, + rate_hz=rate_hz, kp=kp, kd=kd, align=align, + watchdog_s=watchdog_s, dq_max=dq_max) + manager.start() + except BaseException: + self._release_session("teleop") + raise self._teleop = manager self._teleop_transport = created_transport return manager.status() @@ -1909,6 +1961,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() @@ -1923,6 +1976,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 # ═══════════════════════════════════════════════════════════════════ @@ -1935,6 +2277,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() From 17dd21a11f86d5502f9c02f9dcbbe7d0bd38d51d Mon Sep 17 00:00:00 2001 From: cao-xiao-hao <127832643+cao-xiao-hao@users.noreply.github.com> Date: Tue, 29 Sep 2026 16:17:17 +0800 Subject: [PATCH 2/2] test: add an end-to-end trajectory check script The unit suite covers record and replay case by case, but nothing ran the whole path in one go and printed what happened, so verifying the feature by hand meant reading a test file. tests/trajectory_check.py teaches a path, saves it, loads it back and replays it on the recording unit and on a reverse-mounted one, printing every number behind each step and exiting nonzero if any step fails. It is deliberately not named test_* so unittest discover does not collect it twice. The motion comes from tests/fake_can.py and the clock is fake, so the run takes milliseconds and the assertions are equalities rather than tolerances around a scheduler: the capture matches the hand to 2e-16, and the replay matches the trajectory to 5e-14. It proves nothing about real hardware, which still needs a gripper and examples/trajectory.py. --- tests/trajectory_check.py | 522 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 522 insertions(+) create mode 100644 tests/trajectory_check.py 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())