feat: feed the leader velocity forward in gripper teleop

README.md

@@ -167,9 +167,16 @@ python3 examples/teleop.py --mode slave  --channel can0 --host 192.168.1.20
 Both ends must share `grip_id` (default `gripA`) and be connected and enabled first. Teleop is
 exclusive: the background loop owns the CAN I/O, so do not drive the gripper from the caller until
 `teleop_stop()`. `teleop_start` returns the initial `teleop_status()` snapshot; `teleop_status()`
-reports `active`, `mode`, `topic`, `frames`, `last_frame_age_ms`, `stale`, `openness`,
+reports `active`, `mode`, `topic`, `frames`, `last_frame_age_ms`, `stale`, `openness`, `dq_cmd`,
 `loop_hz`, `rejected`, `send_failed`, `fault`, and (master) `matching`.
 
+- **The follower feeds the leader's velocity forward.** The wire frame carries only the opening, so
+  the follower recovers a velocity by differencing successive frames and sends it as the motor's
+  `dq` target — the arm teleoperation sends `dq` outright. Without it the follower biases on
+  position error alone and trails a moving leader (the lag scales with speed / `kp`). Because
+  `kd * dq` is a real torque term the estimate is bounded: the first frame (nothing to difference
+  against), a degenerate interval, and a gap longer than `MAX_FRAME_GAP_S` all yield `dq = 0`, and
+  the result is clamped to `dq_max` (default `10.0` rad/s; `dq_max=0` disables the feedforward).
 - **A follower that loses the leader holds its position, it does not go slack.** After
   `watchdog_s` (default `0.2`) without a fresh frame it keeps commanding its last target under the
   follow gains, so `stale` goes true but the jaws stay put — and can hold whatever is between them.

examples/teleop.py

@@ -40,7 +40,7 @@
 if _SRC not in sys.path:
     sys.path.insert(0, _SRC)
 
-from litegrip import (DEFAULT_GRIP_ID, DEFAULT_GRIP_PORT,  # noqa: E402
+from litegrip import (DEFAULT_DQ_MAX, DEFAULT_GRIP_ID, DEFAULT_GRIP_PORT,  # noqa: E402
                       LiteGrip, LiteGripError)
 
 
@@ -83,6 +83,10 @@ def build_parser() -> argparse.ArgumentParser:
         "--watchdog", type=float, default=0.2,
         help="follower: hold position after this many seconds without a "
              "fresh frame (default: 0.2)")
+    parser.add_argument(
+        "--dq-max", type=float, default=DEFAULT_DQ_MAX,
+        help="follower: ceiling in rad/s on the leader velocity fed forward "
+             f"(default: {DEFAULT_DQ_MAX:.0f}; 0 disables the feedforward)")
     parser.add_argument(
         "--rate", type=float, default=50.0, help="loop rate in Hz (default: 50)")
     parser.add_argument(
@@ -107,7 +111,8 @@ def _print_status(status: dict) -> None:
         extra += f" fault={status['fault']}"
     print(f"frames={status.get('frames', 0):>7} "
           f"age_ms={age_txt} stale={str(status.get('stale', False)):>5} "
-          f"openness={open_txt} "          f"loop_hz={status.get('loop_hz', 0.0):4.1f}"
+          f"openness={open_txt} dq={status.get('dq_cmd', 0.0):+5.2f} "
+          f"loop_hz={status.get('loop_hz', 0.0):4.1f}"
           f"{extra}", flush=True)
 
 
@@ -143,7 +148,7 @@ def main(argv: list[str] | None = None) -> int:
     status = gripper.teleop_start(
         args.mode, link=args.link, host=args.host, port=args.port,
         grip_id=args.grip_id, kp=args.kp, kd=args.kd, align=not args.no_align,
-        watchdog_s=args.watchdog, rate_hz=args.rate)
+        watchdog_s=args.watchdog, dq_max=args.dq_max, rate_hz=args.rate)
     print(f"teleop {args.mode} running; Ctrl+C to stop")
     _print_status(status)
 

readme_zn.md

@@ -151,9 +151,14 @@ python3 examples/teleop.py --mode slave  --channel can0 --host 192.168.1.20
 两端必须共用 `grip_id`（默认 `gripA`），且都已连接、已使能。遥操是互斥的：后台循环独占 CAN
 读写，在 `teleop_stop()` 之前不要再从调用方驱动夹爪。`teleop_start` 返回初始的
 `teleop_status()`；`teleop_status()` 报告 `active`、`mode`、`topic`、`frames`、
-`last_frame_age_ms`、`stale`、`openness`、`loop_hz`、`rejected`、`send_failed`、
+`last_frame_age_ms`、`stale`、`openness`、`dq_cmd`、`loop_hz`、`rejected`、`send_failed`、
 `fault`，主端另有 `matching`。
 
+- **从端把主端的速度前馈下去。** 线上帧只带 openness，所以从端用相邻两帧的差分还原出速度，作为
+  电机的 `dq` 目标下发 —— 机械臂遥操是直接发 `dq` 的。没有这一项，从端只能靠位置误差出力，会
+  明显拖在运动中的主端后面（滞后量 ≈ 速度 / `kp`）。因为 `kd * dq` 是实打实的力矩项，这个估计
+  是有界的：首帧（没有可差分的前一帧）、退化的时间间隔、以及超过 `MAX_FRAME_GAP_S` 的断流都取
+  `dq = 0`，结果再钳到 `dq_max`（默认 `10.0` rad/s；`dq_max=0` 关闭前馈）。
 - **从端与主端失联时是「持位」，不是「卸力」。** 超过 `watchdog_s`（默认 `0.2`）没有新帧后，
   它仍按跟随增益顶着上一个目标继续发帧 —— 于是 `stale` 变真，但爪子停在原地，可能夹住中间的
   东西。

src/litegrip/__init__.py

@@ -124,6 +124,7 @@ def _detect_version(dist_name: str = "litegrip") -> str:
     clamp_to_calibrated,
     DEFAULT_GRIP_ID,
     DEFAULT_GRIP_PORT,
+    DEFAULT_DQ_MAX,
     FRAME_SIZE,
     encode_frame,
     decode_frame,
@@ -211,6 +212,7 @@ def __dir__():
     "clamp_to_calibrated",
     "DEFAULT_GRIP_ID",
     "DEFAULT_GRIP_PORT",
+    "DEFAULT_DQ_MAX",
     "FRAME_SIZE",
     "encode_frame",
     "decode_frame",

src/litegrip/gripper.py

@@ -42,7 +42,7 @@
     MoveProgress,
     MoveResult,
 )
-from .teleop import DEFAULT_GRIP_ID, DEFAULT_GRIP_PORT
+from .teleop import DEFAULT_DQ_MAX, DEFAULT_GRIP_ID, DEFAULT_GRIP_PORT
 
 
 def _zenoh_transport(role: str, key: str, port: int,
@@ -1790,6 +1790,7 @@ def teleop_start(
         kd: Optional[float] = None,
         align: bool = True,
         watchdog_s: float = 0.2,
+        dq_max: float = DEFAULT_DQ_MAX,
         rate_hz: float = 50.0,
     ) -> dict:
         """Start leader/follower teleoperation on this gripper.
@@ -1820,6 +1821,8 @@ def teleop_start(
                 following.
             watchdog_s: Slave only — hold position after this long without a
                 fresh frame.
+            dq_max: Slave only — ceiling in rad/s on the leader velocity fed
+                forward to the follower.  ``0`` disables the feedforward.
             rate_hz: Loop rate.
 
         Returns:
@@ -1866,7 +1869,8 @@ def teleop_start(
 
         manager = GripperTeleop(
             self, transport, mode, key,
-            rate_hz=rate_hz, kp=kp, kd=kd, align=align, watchdog_s=watchdog_s)
+            rate_hz=rate_hz, kp=kp, kd=kd, align=align, watchdog_s=watchdog_s,
+            dq_max=dq_max)
         manager.start()
         self._teleop = manager
         self._teleop_transport = created_transport

src/litegrip/teleop.py

@@ -75,6 +75,18 @@
 DEFAULT_GRIP_ID = "gripA"
 DEFAULT_GRIP_PORT = 17448
 
+#: Default ceiling on the leader velocity the follower feeds forward, in rad/s.
+#: The wire frame carries no velocity, so the follower recovers one by finite
+#: difference (see :meth:`GripperTeleop._estimate_dq`); this bounds what a bad
+#: estimate can demand.  A hand sweep is well under 5 rad/s, and the DM4310's own
+#: ``dq`` range is ±30, so this is generous for motion and tight for a glitch.
+DEFAULT_DQ_MAX = 10.0
+
+#: Longest interval a finite-difference velocity is trusted over, in seconds.
+#: Past this the "velocity" would be an average across a dropout — refuse it and
+#: fall back to position-only control for that cycle.
+MAX_FRAME_GAP_S = 0.05
+
 
 def encode_frame(openness: float, position_mm: float, force_n: float,
                  timestamp: float) -> bytes:
@@ -399,7 +411,8 @@ class GripperTeleop:
     ``mode="master"`` streams zero-torque frames (so the jaws can be moved by
     hand) and publishes the opening.  ``mode="slave"`` subscribes, aligns to
     the first frame with a single :meth:`~litegrip.LiteGrip.goto_rad`, then
-    follows every fresh sample with ``send_mit_frame``.
+    follows every fresh sample with ``send_mit_frame``, feeding the leader's
+    finite-difference velocity forward as ``dq`` (:meth:`_estimate_dq`).
 
     One loop thread per side, sampling and sending in the same cycle — no
     shared buffers and no contention, which is all a single-DOF gripper needs
@@ -420,6 +433,10 @@ class GripperTeleop:
             follower is considered stale and starts holding.  Must be > 0: a
             non-positive watchdog makes the follower permanently stale, which is
             a session that starts and then silently does nothing.
+        dq_max: Slave only — ceiling in rad/s on the leader velocity fed forward
+            as the follower's ``dq`` target.  ``0`` disables the feedforward, in
+            which case the follower biases on position error alone and trails a
+            moving leader.  See :meth:`_estimate_dq`.
         sub_transport: Slave only — a separate transport to subscribe on when
             the leader is remote (the master's transport is local-only).
         sleep_fn, time_fn: Timing seams for tests.  ``time_fn`` must be
@@ -437,6 +454,7 @@ def __init__(
         kd: Optional[float] = None,
         align: bool = True,
         watchdog_s: float = 0.2,
+        dq_max: float = DEFAULT_DQ_MAX,
         sub_transport: Optional[TeleopTransport] = None,
         sleep_fn: Callable[[float], None] = time.sleep,
         time_fn: Callable[[], float] = time.monotonic,
@@ -456,6 +474,7 @@ def __init__(
         self._kd = kd
         self._align = align
         self._watchdog_s = watchdog_s
+        self._dq_max = float(dq_max)
         self._sub_tp = sub_transport
         self._sleep_fn = sleep_fn
         self._time_fn = time_fn
@@ -475,6 +494,12 @@ def __init__(
         self._rejected = 0
         self._send_failed = 0
         self._fault = ""
+        #: Leader velocity fed forward as this cycle's ``dq`` target, and the
+        #: two samples it is differenced from.  Seeded on the first good frame;
+        #: see :meth:`_estimate_dq`.
+        self._dq_cmd = 0.0
+        self._prev_q: Optional[float] = None
+        self._prev_rx_ts: Optional[float] = None
         self._loops = 0
         self._loop_hz = 0.0
         self._hz_t0 = 0.0
@@ -553,6 +578,8 @@ def status(self) -> dict:
             "stale": self._stale,
             "openness": round(self._last_openness, 4),
             "loop_hz": round(self._loop_hz, 1),
+            # Slave only: the leader velocity last fed forward, in rad/s.
+            "dq_cmd": round(self._dq_cmd, 4),
             # Frames dropped at the protocol boundary (non-finite values).
             "rejected": self._rejected,
             # ``send_mit_frame`` returned False — the motor is not following.
@@ -637,6 +664,8 @@ def _slave_loop(self) -> None:
         try:
             while self._running:
                 t0 = self._time_fn()
+                # Only a fresh frame sets a velocity; a hold cycle is ``dq = 0``.
+                dq_cmd = 0.0
                 msg = sub.drain_latest()
                 if msg is not None:
                     try:
@@ -651,6 +680,9 @@ def _slave_loop(self) -> None:
                         else:
                             rx_ts = self._time_fn()
                             q_cmd = openness_to_rad(_clamp01(openness), cfg)
+                            dq_cmd = self._estimate_dq(q_cmd, rx_ts)
+                            self._prev_q = q_cmd
+                            self._prev_rx_ts = rx_ts
                             self._last_openness = _clamp01(openness)
                             self._last_frame_ts = rx_ts
                             self._ever_received = True
@@ -666,8 +698,10 @@ def _slave_loop(self) -> None:
                 # Always send — including while stale.  The frame both holds
                 # the position and keeps the motor from self-locking.
                 q_cmd = clamp_to_calibrated(cfg, q_cmd)
+                self._dq_cmd = dq_cmd
                 try:
-                    self._send(q_cmd, self._resolve_kp(), self._resolve_kd())
+                    self._send(q_cmd, self._resolve_kp(), self._resolve_kd(),
+                               dq=dq_cmd)
                     self._note_grip_fault(self._g.get_state(wait=False))
                 except Exception:  # noqa: BLE001
                     log.exception("[slave] CAN error; loop exiting")
@@ -710,6 +744,31 @@ def _wait_first_frame(self, sub: TeleopSubscription, timeout_s: float,
             self._sleep_fn(0.01)
         return None
 
+    def _estimate_dq(self, q: float, rx_ts: float) -> float:
+        """Leader velocity in rad/s, differenced from the previous good frame.
+
+        The gripper wire frame carries no velocity field, so the follower
+        recovers one here and feeds it forward as its own ``dq`` target — this
+        stands in for the ``dq`` the arm teleoperation sends outright.  Without
+        it the follower biases on position error alone and visibly trails a
+        moving leader (the lag scales with speed / ``kp``).
+
+        Deliberately conservative: nothing to difference against on the first
+        frame, a degenerate or dropout-sized interval is refused, and the result
+        is clamped to ``dq_max``.  ``kd * (dq - dq_measured)`` is a real torque
+        term, so an unbounded estimate could command a large one.
+        """
+        if (self._dq_max <= 0.0 or self._prev_q is None
+                or self._prev_rx_ts is None):
+            return 0.0
+        dt = rx_ts - self._prev_rx_ts
+        if not 1e-4 < dt < MAX_FRAME_GAP_S:
+            return 0.0
+        dq = (q - self._prev_q) / dt
+        if not math.isfinite(dq):
+            return 0.0
+        return max(-self._dq_max, min(self._dq_max, dq))
+
     def _count_rejected(self, got: Any) -> None:
         """Record a frame dropped at the protocol boundary (§8 rule 9)."""
         self._rejected += 1
@@ -718,15 +777,19 @@ def _count_rejected(self, got: Any) -> None:
                         "holding position rather than folding onto a stop",
                         self._mode, got)
 
-    def _send(self, q: float, kp: float, kd: float,
+    def _send(self, q: float, kp: float, kd: float, dq: float = 0.0,
               quiet: bool = False) -> None:
         """Send one MIT frame and **check the return value** (§8 rule 10).
 
+        ``dq`` is the follower's velocity target: zero for a hold (and for the
+        master's slack frames), the leader's finite-difference velocity for a
+        follower cycle — see :meth:`_estimate_dq`.
+
         ``send_mit_frame`` returns ``False`` when the motor is not enabled or the
         CAN write fails — it does not raise, so ignoring the result means
         believing we are driving a gripper that is not moving.
         """
-        if not self._g.send_mit_frame(q=q, kp=kp, kd=kd):
+        if not self._g.send_mit_frame(q=q, kp=kp, kd=kd, dq=dq):
             self._send_failed += 1
             if self._send_failed == 1 and not quiet:
                 log.warning("[%s] send_mit_frame returned False — the motor is "

tests/test_teleop.py

@@ -18,11 +18,11 @@
 import unittest.mock
 
 import _sdkpath  # noqa: F401
-from litegrip import (FRAME_SIZE, GripperTeleop,
+from litegrip import (DEFAULT_DQ_MAX, FRAME_SIZE, GripperTeleop,
                       InProcTeleopTransport, TeleopBusyError, TeleopNotReady,
                       UdpTeleopTransport, decode_frame, encode_frame,
                       teleop_topic)
-from litegrip.teleop import (clamp_to_calibrated, check_ready,
+from litegrip.teleop import (MAX_FRAME_GAP_S, clamp_to_calibrated, check_ready,
                              openness_to_rad, rad_to_openness, travel_mm)
 
 from fake_can import POS_CLOSED_RAD, POS_OPEN_RAD, RAD_TO_MM, make_gripper
@@ -454,5 +454,76 @@ def test_rejects_non_positive_watchdog(self):
                           watchdog_s=0.0)
 
 
+class VelocityFeedforwardTest(unittest.TestCase):
+    """The leader's velocity is recovered locally and fed forward as ``dq``.
+
+    The gripper wire frame carries no velocity field, so the follower
+    differences successive positions.  These bounds keep a bad estimate from
+    turning into a large ``kd * (dq - dq_measured)`` torque.
+    """
+
+    def _mgr(self, **kwargs):
+        g, _ = make_gripper()
+        return GripperTeleop(g, InProcTeleopTransport(), "slave", TOPIC,
+                             sleep_fn=_nop_sleep, **kwargs)
+
+    def test_first_sample_has_nothing_to_difference(self):
+        self.assertEqual(self._mgr()._estimate_dq(0.5, 100.0), 0.0)
+
+    def test_is_a_finite_difference(self):
+        mgr = self._mgr()
+        mgr._prev_q, mgr._prev_rx_ts = 0.10, 100.00
+        self.assertAlmostEqual(mgr._estimate_dq(0.15, 100.01), 5.0)
+
+    def test_is_clamped_to_dq_max(self):
+        mgr = self._mgr(dq_max=2.0)
+        mgr._prev_q, mgr._prev_rx_ts = 0.0, 100.0
+        self.assertEqual(mgr._estimate_dq(5.0, 100.01), 2.0)
+        self.assertEqual(mgr._estimate_dq(-5.0, 100.01), -2.0)
+
+    def test_refuses_a_dropout_sized_gap(self):
+        mgr = self._mgr()
+        mgr._prev_q, mgr._prev_rx_ts = 0.0, 100.0
+        self.assertEqual(
+            mgr._estimate_dq(0.5, 100.0 + MAX_FRAME_GAP_S + 0.01), 0.0)
+
+    def test_refuses_a_degenerate_interval(self):
+        mgr = self._mgr()
+        mgr._prev_q, mgr._prev_rx_ts = 0.0, 100.0
+        self.assertEqual(mgr._estimate_dq(0.5, 100.0 + 1e-6), 0.0)
+
+    def test_zero_dq_max_disables_the_feedforward(self):
+        mgr = self._mgr(dq_max=0.0)
+        mgr._prev_q, mgr._prev_rx_ts = 0.0, 100.0
+        self.assertEqual(mgr._estimate_dq(0.5, 100.01), 0.0)
+
+    def test_slave_sends_the_leader_velocity_as_dq(self):
+        # Paced by the real clock, so consecutive frames land a realistic
+        # interval apart.  The step is then far past ``dq_max`` and clamps,
+        # which is stable against the exact timing.
+        g, fake = make_gripper(start_rad=POS_CLOSED_RAD, reverse=False)
+        transport = _PreSubTransport()
+        mgr = GripperTeleop(g, transport, "slave", TOPIC, rate_hz=100.0,
+                            align=False, watchdog_s=5.0)
+        transport.pub(TOPIC, encode_frame(0.2, 24.0, 0.0, 0.0))
+        mgr.start()
+        try:
+            q1 = openness_to_rad(0.2, g.config)
+            self.assertTrue(_wait_until(
+                lambda: fake.frames and abs(fake.frames[-1].q - q1) < 1e-9))
+            # Nothing to difference against yet: a pure position hold.
+            self.assertEqual(fake.frames[-1].dq, 0.0)
+            transport.pub(TOPIC, encode_frame(0.8, 96.0, 0.0, 0.0))
+            q2 = openness_to_rad(0.8, g.config)
+            self.assertTrue(_wait_until(
+                lambda: any(abs(f.q - q2) < 1e-9 for f in fake.frames)))
+            # Opening up drives the normal-mount jaw toward a smaller angle, so
+            # the fed-forward velocity is negative — and bounded.
+            self.assertTrue(any(abs(f.q - q2) < 1e-9 and f.dq == -DEFAULT_DQ_MAX
+                                for f in fake.frames))
+        finally:
+            mgr.stop()
+
+
 if __name__ == "__main__":
     unittest.main()