Skip to content

feat: feed the leader velocity forward in gripper teleop - #16

Merged
yd-sl merged 1 commit into
mainfrom
feat/teleop-velocity-feedforward
Sep 29, 2026
Merged

yd-sl merged 1 commit into
mainfrom
feat/teleop-velocity-feedforward

Conversation

@yd-sl

@yd-sl yd-sl commented Sep 29, 2026

Copy link
Copy Markdown
Contributor

Summary

The follower felt laggy next to the arm teleoperation, and the reason turned out to be structural rather than a tuning problem.

The arm's MIT command is kp*(q_cmd - q) + kd*(dq_cmd - dq) + tau_ff with a real dq_cmd, and the arm link runs at a higher rate. The gripper wire frame carries only an opening — no velocity. With no velocity term the follower leans on position error alone, so it trails a moving leader by roughly speed / kp. That is felt as drag, and it is worst during a fast hand sweep. Raising kp and the loop rate reduced it but could not remove it.

The wire format cannot change: it is byte-identical to the litearm stack and interoperates with it. So the follower recovers the leader's velocity locally by differencing successive positions and sends the result as its own dq target.

kd * (dq_cmd - dq) is a genuine torque term, so the estimate is bounded on purpose: the first frame has nothing to difference against, a degenerate interval and a gap longer than MAX_FRAME_GAP_S (0.05 s — longer than that and the "velocity" is a dropout average, not a velocity) are both refused, and the result is clamped to dq_max (default 10.0 rad/s). dq_max=0 disables the feedforward for anyone who wants the old behaviour.

Changes

  • src/litegrip/teleop.py — DEFAULT_DQ_MAX = 10.0 and MAX_FRAME_GAP_S = 0.05; GripperTeleop(dq_max=...); _estimate_dq(); the slave loop differences a fresh frame and passes the result as the dq of its MIT frame (a hold cycle sends dq = 0); _send() gains a dq parameter; status() reports dq_cmd.
  • src/litegrip/gripper.py — teleop_start(..., dq_max=DEFAULT_DQ_MAX) passes it through.
  • src/litegrip/__init__.py — export DEFAULT_DQ_MAX.
  • examples/teleop.py — --dq-max (0 disables) and dq= in the status line.
  • tests/test_teleop.py — VelocityFeedforwardTest: nothing to difference on the first sample, a finite difference, clamping at dq_max, refusal of a dropout-sized gap and of a degenerate interval, dq_max=0 disabling it, and an end-to-end check that the slave puts the leader velocity on the wire as dq with the right sign.
  • README.md / readme_zn.md — the feedforward note and dq_cmd in the status list.

Testing

$ cd /home/qql/sl/litegrip-python && python3 -m unittest discover -s tests -t tests
Ran 149 tests in 1.888s

OK

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

Also run with the zenoh module blocked (the CI machine has no zenoh): Ran 149 tests ... OK (skipped=12).

Hardware validation, 2026-09-29, leader on can0 / follower on can1, hand-driven jaws:

  • The follower tracks the leader in both directions; the drag felt before this change is gone.
  • At rest the fed-forward dq sits at a noise floor of about ±0.04 rad/s, roughly 0.08 Nm of extra torque — small enough that the loop is not audibly or visibly jittery.
  • No rejected and no fault over the runs.

Issues

None — the repository has no issue tracker entries, matching the previous PRs.

The follower felt laggy next to the arm teleoperation, and the reason is
structural: the arm's MIT command is kp*(q_cmd - q) + kd*(dq_cmd - dq) +
tau_ff with a real dq_cmd, while the gripper wire frame carries only an
opening. With no velocity term the follower leans on position error alone,
so it trails a moving leader by roughly speed / kp -- felt as drag, worst
during a fast hand sweep.

The wire format cannot change (it is byte-identical to the litearm stack and
interoperates with it), so the follower recovers the leader's velocity
locally: it differences successive positions and sends the result as its own
dq target. `kd * (dq_cmd - dq)` is a genuine torque term, so the estimate is
bounded deliberately: the first frame has nothing to difference against, a
degenerate interval and a gap longer than MAX_FRAME_GAP_S are both refused,
and the result is clamped to `dq_max` (default 10.0 rad/s; `dq_max=0`
disables the feedforward).

Confirmed on hardware with a hand sweep: the drag is gone and the loop stays
quiet -- a resting `dq` noise floor of about ±0.04 rad/s, roughly 0.08 Nm.
@yd-sl
yd-sl merged commit 5ed7175 into main Sep 29, 2026
1 check passed
@yd-sl
yd-sl deleted the feat/teleop-velocity-feedforward branch September 29, 2026 07:51
@github-actions

Copy link
Copy Markdown

🎉 This PR is included in version 0.7.0 🎉

The release is available on GitHub release

Your semantic-release bot 📦🚀

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

Labels

Projects

None yet

Development

Successfully merging this pull request may close these issues.

1 participant