From e66b4c6e11cf4f01d572638dbd445c23e07cd8aa Mon Sep 17 00:00:00 2001 From: yd-sl <166189879+yd-sl@users.noreply.github.com> Date: Tue, 29 Sep 2026 15:50:19 +0800 Subject: [PATCH] feat: feed the leader velocity forward in gripper teleop MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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. --- README.md | 9 ++++- examples/teleop.py | 11 ++++-- readme_zn.md | 7 +++- src/litegrip/__init__.py | 2 ++ src/litegrip/gripper.py | 8 +++-- src/litegrip/teleop.py | 71 ++++++++++++++++++++++++++++++++++--- tests/test_teleop.py | 75 ++++++++++++++++++++++++++++++++++++++-- 7 files changed, 170 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index af81584..42b574d 100644 --- a/README.md +++ b/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. diff --git a/examples/teleop.py b/examples/teleop.py index 625cb06..4b59479 100644 --- a/examples/teleop.py +++ b/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) diff --git a/readme_zn.md b/readme_zn.md index 2f4a16a..4ef93a8 100644 --- a/readme_zn.md +++ b/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` 变真，但爪子停在原地，可能夹住中间的 东西。 diff --git a/src/litegrip/__init__.py b/src/litegrip/__init__.py index fcec1c3..11c5c3c 100644 --- a/src/litegrip/__init__.py +++ b/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", diff --git a/src/litegrip/gripper.py b/src/litegrip/gripper.py index 70cf451..aa98930 100644 --- a/src/litegrip/gripper.py +++ b/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 diff --git a/src/litegrip/teleop.py b/src/litegrip/teleop.py index 5a3d8d2..a2f3a6c 100644 --- a/src/litegrip/teleop.py +++ b/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 " diff --git a/tests/test_teleop.py b/tests/test_teleop.py index 3db9495..3bbfe52 100644 --- a/tests/test_teleop.py +++ b/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()