Skip to content

feat: hold the grasp on over-torque and return the force to the leader - #81

Merged
cao-xiao-hao merged 2 commits into
mainfrom
feat/gripper-hold-and-force-feedback
Nov 12, 2026
Merged

cao-xiao-hao merged 2 commits into
mainfrom
feat/gripper-hold-and-force-feedback

Conversation

@yd-sl

@yd-sl yd-sl commented Nov 12, 2026

Copy link
Copy Markdown
Contributor

Summary

An over-torque follower used to go slack in place, so the part it had just clamped pushed the jaws open — the gripper could not hold anything. It now keeps pressing with a bounded pure-torque frame. The leader can also feel the grasp: the follower reports its squeeze and the leader renders it as a resistance torque, so the operator no longer has to judge the grasp by eye alone.

The implementation follows the hardware-validated force path rather than inventing a new one: the hold frame has the same shape as GripperActions._hold_force, and send_mit_frame(tau=...) is the documented call for force feedback.

Changes

  • teleop.py: on a torque-guard trip, latch the sign of the measured torque and send kp=kd=0, tau=±torque_limit_nm instead of a zero-stiffness frame. The previous behaviour is available as torque_guard_action="release". Applied to the follow loop, the align ramp, and the handover so all three stay consistent.
  • teleop.py: add the feedback codec (>d, 8 bytes), feedback_topic(), and the leader-side render (threshold → gain → cap, signed to oppose closing, damping only while the force is fresh, both torque and damping zero on timeout / never received).
  • teleop.py: drain the ready and feedback subscriptions every cycle (the ready drain used to run first and never again), and expose feedback_force_n, feedback_tau_nm, feedback_rx, feedback_tx from status().
  • zenoh_link.py: carry the third key through Listener, Connector, and ZenohTeleopTransport; feedback_key=None is byte-for-byte the previous behaviour.
  • gripper.py / __init__.py: accept and forward the new teleop_start arguments and the feedback key, and export the new constants, codec, and topic.
  • README.md, README.zh-CN.md: document the hold semantics, the feedback key and defaults, and the automatic degradation (older peer, UDP leader).

Testing

python3 -m unittest discover -s tests -t tests
Ran 498 tests in 10.407s

OK

New tests cover both mounts (the hold direction follows the torque measured at the trip, not close_sign), the release option, the align and handover paths, the codec boundaries (wrong length, non-finite), the leader render (fresh / below threshold / capped / stale / off by default), a publish-only UDP leader, and the follower's mount-normalised publish.

npx markdownlint-cli2@0.23.3 README.md README.zh-CN.md reports no issues.

Issues

No linked issue — requested from the field: the follower could not hold a part, and the leader had no feel for the grasp.

Checklist

  • The title follows Conventional Commits and matches the commit subject.
  • The body is written in English.
  • Tests were run and the result is stated above.
  • Human-facing documents are paired with .zh-CN.md and open with the language switcher.
  • No secrets, credentials, or private URLs are included.
  • Closes is used only when the pull request fully resolves the issue.

An over-torque follower used to drop to zero stiffness and damping in place,
so the clamped part pushed the jaws straight open — the gripper could not
hold anything. It now keeps pressing with a bounded pure-torque frame
(kp=kd=0, tau=+/-torque_limit_nm, signed by the torque measured at the trip),
the same shape as the hardware-validated GripperActions._hold_force. The old
behaviour is still available as torque_guard_action="release".

The leader can now feel the grasp: the follower publishes its mount-normalised
squeeze on its own zenoh key (litearm/v4/{grip_id}/gripper_feedback, an 8-byte
double, additive so older peers degrade to position-only teleop) and the leader
renders a resistance torque above a threshold, capped, with damping only while
the force is fresh. The forward and ready frames are untouched. UDP has no
receive path on the leader, so feedback turns itself off there.

Both mounts are covered by tests: the hold direction follows the measured
torque sign, not close_sign, and a wrong leader sign would drive the grasp
closed instead of resisting it.
Reading status() after the publish loop exits is a race: the master loop
keeps spinning with no further frames, so the 0.3 s timeout zeroes
feedback_tau_nm before the assertion reads it. Take the status sample
inside the predicate, while feedback is still being published. Seen on CI.
@cao-xiao-hao
cao-xiao-hao merged commit 66b7c16 into main Nov 12, 2026
1 check passed
@cao-xiao-hao
cao-xiao-hao deleted the feat/gripper-hold-and-force-feedback branch November 12, 2026 06:34
@github-actions

Copy link
Copy Markdown

🎉 This PR is included in version 1.3.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

released Shipped in a release

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants