diff --git a/README.md b/README.md index 42b574d..c603484 100644 --- a/README.md +++ b/README.md @@ -167,8 +167,10 @@ 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`, `dq_cmd`, -`loop_hz`, `rejected`, `send_failed`, `fault`, and (master) `matching`. +reports `active`, `mode`, `topic`, `frames`, `last_frame_age_ms`, `stale`, `openness`, +`position_mm`, `force_n`, `dq_cmd`, `loop_hz`, `rejected`, `send_failed`, `fault`, and (master) +`matching`. `position_mm` and `force_n` are the master's own state, or the leader's values from +the frame the slave followed — a caller can show the jaws without opening a second CAN reader. - **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 diff --git a/readme_zn.md b/readme_zn.md index 4ef93a8..b2a2608 100644 --- a/readme_zn.md +++ b/readme_zn.md @@ -151,8 +151,9 @@ 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`、`dq_cmd`、`loop_hz`、`rejected`、`send_failed`、 -`fault`，主端另有 `matching`。 +`last_frame_age_ms`、`stale`、`openness`、`position_mm`、`force_n`、`dq_cmd`、`loop_hz`、 +`rejected`、`send_failed`、`fault`，主端另有 `matching`。`position_mm` 与 `force_n` 是主端自 +己的状态，或从端刚刚跟随的那一帧里主端的值 —— 调用方不必再开一路 CAN 读就能显示夹爪。 - **从端把主端的速度前馈下去。** 线上帧只带 openness，所以从端用相邻两帧的差分还原出速度，作为 电机的 `dq` 目标下发 —— 机械臂遥操是直接发 `dq` 的。没有这一项，从端只能靠位置误差出力，会 diff --git a/src/litegrip/teleop.py b/src/litegrip/teleop.py index a2f3a6c..db3df24 100644 --- a/src/litegrip/teleop.py +++ b/src/litegrip/teleop.py @@ -485,6 +485,13 @@ def __init__( # Diagnostics. self._frames = 0 self._last_openness = 0.0 + #: Last ``position_mm`` / ``force_n`` the session saw — the master reads + #: them off its own state, the slave takes them from the frame it + #: followed. Reported by :meth:`status` so a caller can show what the + #: jaws are doing without a second CAN reader (the teleop loop owns the + #: bus while it runs). + self._last_position_mm = 0.0 + self._last_force_n = 0.0 self._last_frame_ts = 0.0 #: Explicit "have we ever received a frame" flag. The watchdog only #: applies after the first frame, and this must not be inferred from a @@ -577,6 +584,10 @@ def status(self) -> dict: "last_frame_age_ms": age_ms, "stale": self._stale, "openness": round(self._last_openness, 4), + # Position and force behind that opening — the master's own state, + # or (slave) the leader's values from the followed frame. + "position_mm": round(self._last_position_mm, 4), + "force_n": round(self._last_force_n, 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), @@ -617,6 +628,8 @@ def _master_loop(self) -> None: self._count_rejected(openness) else: self._last_openness = openness + self._last_position_mm = float(state.position_mm) + self._last_force_n = float(state.force_n) try: self._tp.pub(self._topic, encode_frame( openness, state.position_mm, state.force_n, @@ -657,6 +670,8 @@ def _slave_loop(self) -> None: self._last_frame_ts = self._time_fn() self._ever_received = True self._last_openness = _clamp01(first[0]) + self._last_position_mm = float(first[1]) + self._last_force_n = float(first[2]) else: log.warning("[slave] no frame within align timeout; " "holding current position") @@ -684,6 +699,8 @@ def _slave_loop(self) -> None: self._prev_q = q_cmd self._prev_rx_ts = rx_ts self._last_openness = _clamp01(openness) + self._last_position_mm = float(_mm) + self._last_force_n = float(_force) self._last_frame_ts = rx_ts self._ever_received = True self._frames += 1 diff --git a/tests/test_teleop.py b/tests/test_teleop.py index 3bbfe52..08ad359 100644 --- a/tests/test_teleop.py +++ b/tests/test_teleop.py @@ -178,6 +178,25 @@ def test_publishes_openness_and_zero_torque(self): finally: mgr.stop() + def test_status_reports_position_and_force(self): + # A caller (an operator UI) shows the jaws' position and force from the + # status alone — it cannot read the bus itself, the loop owns it. + g, fake = make_gripper(start_rad=POS_OPEN_RAD) + fake.motor.step = lambda *a, **k: None + transport = InProcTeleopTransport() + sub = transport.sub(TOPIC) + mgr = GripperTeleop(g, transport, "master", TOPIC, rate_hz=200.0, + sleep_fn=_nop_sleep) + mgr.start() + try: + self.assertTrue(_wait_until(lambda: mgr.status()["frames"] > 0)) + _o, position_mm, force_n, _ts = decode_frame(sub.drain_latest()) + status = mgr.status() + self.assertAlmostEqual(status["position_mm"], position_mm, places=3) + self.assertAlmostEqual(status["force_n"], force_n, places=3) + finally: + mgr.stop() + def test_stop_leaves_zero_gravity(self): g, fake = make_gripper() mgr = GripperTeleop(g, InProcTeleopTransport(), "master", TOPIC, @@ -213,6 +232,19 @@ def test_follows_published_openness(self): finally: mgr.stop() + def test_status_reports_the_followed_position_and_force(self): + g, fake, transport, mgr = self._slave(align=False, watchdog_s=5.0) + transport.pub(TOPIC, encode_frame(0.25, 30.0, -4.5, 0.0)) + mgr.start() + try: + self.assertTrue(_wait_until(lambda: mgr.status()["frames"] > 0)) + status = mgr.status() + self.assertAlmostEqual(status["openness"], 0.25, places=4) + self.assertAlmostEqual(status["position_mm"], 30.0, places=4) + self.assertAlmostEqual(status["force_n"], -4.5, places=4) + finally: + mgr.stop() + def test_watchdog_holds_instead_of_relaxing(self): g, fake, transport, mgr = self._slave(align=False, watchdog_s=0.05) transport.pub(TOPIC, encode_frame(0.8, 96.0, 0.0, 0.0))