Skip to content

feat: add gripper trajectory record and replay - #13

Closed
cao-xiao-hao wants to merge 2 commits into
mainfrom
feat/gripper-trajectory-record-replay
Closed

cao-xiao-hao wants to merge 2 commits into
mainfrom
feat/gripper-trajectory-record-replay

Conversation

@cao-xiao-hao

Copy link
Copy Markdown
Contributor

Summary

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 written as code, with no way to capture one off the hardware.

This adds recording and replay. record() puts the motor into zero-gravity so the jaws can be pushed by hand, samples the opening, and returns a Trajectory that can be saved to a file; play() streams the captured openings back as MIT command frames. The reference is litearm-core's log module, and the three rules it enforces are ported rather than documented: never return half data as a success, reject a stream whose length is not an exact multiple of the sample size, and refuse to loop forever on a source that stopped advancing — the last one applied to the sample clock in both directions.

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 taught against an object repeats as a position path and not as the same grip force.

This is additive. No existing signature, behaviour or default changes, so nothing breaks for consumers and the change publishes as a minor under the repository's version policy.

Changes

  • New src/litegrip/trajectory.py — TrajectorySample (frozen dataclass: t, openness, position_rad, velocity_rad_s, torque_nm), Trajectory (samples plus provenance, openness_at, to_bytes/from_bytes, save/load), TrajectoryRecorder, TrajectoryPlayer, and the error hierarchy under LiteGripError.
    • Compact binary format: 66-byte self-describing header (LGRTRJ01 magic, version, sample count, sample_hz, created, the unit's closed/open limits and rad_to_mm, can_id, mount) followed by 40-byte little-endian samples. from_bytes rejects wrong magic, unknown version, a length that is not exactly 66 + n*40 (trailing bytes included), non-positive sample_hz, zero travel, non-finite numbers, non-monotonic t, and an undecodable mount.
    • The recorder captures sample 0 synchronously before the thread starts, streams (kp, kd) == (0, 0) every cycle in zero-gravity mode so the motor never self-locks its comm-loss fault mid-teach, and leaves zero-gravity in a finally so a failed capture cannot leave the jaws slack. record_stop() raises with the sample count when the loop died and raises TrajectoryEmptyError when nothing landed.
    • The player is wall-clock driven (elapsed = (now - t0) * speed), so a slow cycle skips ahead instead of stretching the motion; align does one goto_rad to sample 0 first; loop wraps by whole loops so an overrunning cycle keeps its phase.
  • src/litegrip/gripper.py — seven methods on LiteGrip in the shape of the teleop block: record, record_start, record_stop, play, play_start, play_stop, trajectory_status. Adds _session_lock with _claim_session/_release_session so recording, replay and teleoperation cannot interleave frames on one motor; teleop_start now rejects a running recording and disconnect() tears a trajectory session down. Extracts the hold-position frame shared by the teleop master and a stopping replay into _hold_position().
  • src/litegrip/__init__.py — exports Trajectory, TrajectorySample, TrajectoryRecorder, TrajectoryPlayer, trajectory_dir, resolve_path, DEFAULT_RATE_HZ and the five error classes.
  • New tests/test_trajectory.py — 74 hardware-free tests against tests/fake_can.py, with the timing seams stubbed and threads driven by a deterministic fake clock.
  • New examples/trajectory.py — --record SECONDS --save NAME, --play NAME, --list, same argparse + --dry-run shape as examples/teleop.py.
  • README.md / readme_zn.md — a "Trajectory record and replay" / "轨迹录制与回放" section in both languages, with a quick start, the method table, CLI examples, the file and path rules, and the warnings that matter (position not force; jaws are slack during record(); only one hold frame from a blocking play(); recording, replay and teleop are mutually exclusive; the unit must be calibrated).

Testing

$ python3 -m unittest discover -s tests -t tests
...................................................................................................................................................................................
----------------------------------------------------------------------
Ran 179 tests in 2.082s

OK

Stable across repeated runs. The suite is hardware-free, so green here is the whole check — tests/fake_can.py simulates the motor kinematics and the CAN layer.

$ npx --yes markdownlint-cli2@0.23.3 "README.md" "readme_zn.md"
Summary: 0 issues in 0 files

CLI, run without hardware under a scratch trajectory directory:

$ PYTHONPATH=src LITEGRIP_TRAJ_DIR=/tmp/lg-demo python3 examples/trajectory.py --record 3 --save demo --dry-run
$ PYTHONPATH=src LITEGRIP_TRAJ_DIR=/tmp/lg-demo python3 examples/trajectory.py --play demo --repeat 1 --dry-run
$ PYTHONPATH=src LITEGRIP_TRAJ_DIR=/tmp/lg-demo python3 examples/trajectory.py --list

--list also tolerates one unreadable file: a deliberately truncated broken.lgt reports broken.lgt: unreadable (文件只有 7B, 连 66B 的头都不够) while still listing demo.lgt. --dry-run proves the CLI builds its plan without a CAN interface; it does not prove motion, which needs a real gripper.

Issues

No linked issue — gh issue list --search "trajectory OR record OR replay" --state all found none, and this was requested directly.

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.
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.
@cao-xiao-hao

Copy link
Copy Markdown
Contributor Author

Superseded by #18. main moved on by four commits (#14–#17) while this was open, so the branch conflicted; #18 carries the same two commits rebased onto main, with the conflicts resolved and the full suite re-run there.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant