Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 19 additions & 7 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -513,9 +513,9 @@ with LiteGrip("can0") as slave:

| Method | What it does |
| --- | --- |
| `teleop_start(mode, *, transport=None, link="zenoh", host=None, port=17448, grip_id="gripA", kp=None, kd=None, align=True, watchdog_s=0.2, dq_max=10.0, torque_limit_nm=0.0, require_ready=True, ...)` → `dict` | Starts the background loop for `mode="master"` or `"slave"`. Both ends must share `grip_id` and be connected and enabled first. |
| `teleop_start(mode, *, transport=None, link="zenoh", host=None, port=17448, grip_id="gripA", kp=None, kd=None, align=True, watchdog_s=0.2, dq_max=10.0, torque_limit_nm=0.0, torque_guard_action="hold", require_ready=True, force_feedback=False, feedback_gain_nm_per_n=0.10, ...)` → `dict` | Starts the background loop for `mode="master"` or `"slave"`. Both ends must share `grip_id` and be connected and enabled first. |
| `teleop_stop(timeout=2.0)` → `dict` | Stops the loop and leaves the jaws holding. |
| `teleop_status()` → `dict` | `active`, `mode`, `frames`, `last_frame_age_ms`, `stale`, `openness`, `position_mm`, `force_n`, `loop_hz`, `rejected`, `send_failed`, `fault`, `torque_nm`, `over_torque`, `torque_trips`, `ready`, and more. |
| `teleop_status()` → `dict` | `active`, `mode`, `frames`, `last_frame_age_ms`, `stale`, `openness`, `position_mm`, `force_n`, `loop_hz`, `rejected`, `send_failed`, `fault`, `torque_nm`, `over_torque`, `torque_trips`, `ready`, `feedback_force_n`, `feedback_tau_nm`, `feedback_rx`, `feedback_tx`, and more. |

The link is a point-to-point **zenoh** session, an optional dependency:

Expand Down Expand Up @@ -545,11 +545,23 @@ python3 examples/teleop.py --mode slave --channel can0 --fake-leader --torque-li
- **The leader holds its jaws under gain until the follower reports ready**, so the operator
cannot push the target out from under a follower that is still travelling. `ready_timeout_s`
(default `10.0`) bounds the wait; `require_ready=False` restores the immediate hand-back.
- **An optional torque guard releases a follower that is pressing too hard.** Set
`torque_limit_nm` and it goes to zero stiffness and damping *in place* after 60 ms over the
limit, then re-arms once the leader has reopened. The default is `0` — **the guard is off
unless you ask for it**. Pick the value per machine: it depends on how fragile the part between
the jaws is.
- **An optional torque guard stops a follower that is pressing too hard.** Set
`torque_limit_nm` and, after 60 ms over the limit, the follower stops following and **keeps
pressing with that torque** (`kp=kd=0`, `tau=±torque_limit_nm`, signed by the torque measured
at the trip) so a clamped part is not pushed open; it re-arms once the leader has reopened.
`torque_guard_action="release"` restores the previous behaviour — zero stiffness and damping
*in place* — which lets the part push the jaws straight open. The default is `0` — **the guard
is off unless you ask for it**. Pick the value per machine: it depends on how fragile the part
between the jaws is.
- **The follower's squeeze can be felt on the leader.** With `force_feedback=True` the follower
publishes `force_n` on `litearm/v4/{grip_id}/gripper_feedback` (positive means closing,
normalised for the mount) and the leader renders resistance at
`min(max(0, force_n - feedback_threshold_n) * feedback_gain_nm_per_n, feedback_max_tau_nm)`,
directed against closing. Defaults: threshold `2.0` N, gain `0.10` Nm/N, cap `1.0` Nm.
Damping (`0.1`) applies only while the force is fresh; torque and damping both drop to zero
after `feedback_timeout_s` (`0.3`). It is off by default and needs the zenoh link — a UDP
leader is publish-only, so it runs without feedback. The key is additive, so an older peer
simply runs position-only teleoperation.
- **A follower that loses the leader holds its position, it does not go slack.** After
`watchdog_s` (default `0.2`) without a fresh frame, `stale` goes true and the jaws stay put.
- **The follower feeds the leader's velocity forward** by differencing successive frames, so it
Expand Down
20 changes: 15 additions & 5 deletions README.zh-CN.md
Original file line number Diff line number Diff line change
Expand Up @@ -446,9 +446,9 @@ with LiteGrip("can0") as slave:

| 方法 | 做什么 |
| --- | --- |
| `teleop_start(mode, *, transport=None, link="zenoh", host=None, port=17448, grip_id="gripA", kp=None, kd=None, align=True, watchdog_s=0.2, dq_max=10.0, torque_limit_nm=0.0, require_ready=True, ...)` → `dict` | 为 `mode="master"` 或 `"slave"` 起后台循环。两端必须共用 `grip_id`,并且都已连接、已使能。 |
| `teleop_start(mode, *, transport=None, link="zenoh", host=None, port=17448, grip_id="gripA", kp=None, kd=None, align=True, watchdog_s=0.2, dq_max=10.0, torque_limit_nm=0.0, torque_guard_action="hold", require_ready=True, force_feedback=False, feedback_gain_nm_per_n=0.10, ...)` → `dict` | 为 `mode="master"` 或 `"slave"` 起后台循环。两端必须共用 `grip_id`,并且都已连接、已使能。 |
| `teleop_stop(timeout=2.0)` → `dict` | 停掉循环,爪子保持在当前位置。 |
| `teleop_status()` → `dict` | `active`、`mode`、`frames`、`last_frame_age_ms`、`stale`、`openness`、`position_mm`、`force_n`、`loop_hz`、`rejected`、`send_failed`、`fault`、`torque_nm`、`over_torque`、`torque_trips`、`ready` 等。 |
| `teleop_status()` → `dict` | `active`、`mode`、`frames`、`last_frame_age_ms`、`stale`、`openness`、`position_mm`、`force_n`、`loop_hz`、`rejected`、`send_failed`、`fault`、`torque_nm`、`over_torque`、`torque_trips`、`ready`、`feedback_force_n`、`feedback_tau_nm`、`feedback_rx`、`feedback_tx` 等。 |

链路是**点对点 zenoh**,属于可选依赖:

Expand Down Expand Up @@ -476,9 +476,19 @@ python3 examples/teleop.py --mode slave --channel can0 --fake-leader --torque-li
- **从端报告就位之前,主端一直带着增益顶着自己的爪子**,所以操作者无法在一个还在移动的从端
下面把目标推走。等待由 `ready_timeout_s`(默认 `10.0`)兜底;`require_ready=False` 恢复
立即交还。
- **可选的力矩护栏会放掉压得太狠的从端。** 设了 `torque_limit_nm` 之后,超过限值 60 ms 它就
**就地**把刚度和阻尼降到零,等主端重新张开后重新武装。默认是 `0` —— **不显式要求就不启用**。
这个值要按机器挑:它取决于爪子之间那个工件有多脆。
- **可选的力矩护栏会拦住压得太狠的从端。** 设了 `torque_limit_nm` 之后,超过限值 60 ms 它就
停止跟随,**改用该限值继续压住**(`kp=kd=0`、`tau=±torque_limit_nm`,方向取触发瞬间实测
力矩的符号),被夹住的工件因此不会被钳口顶开;等主端重新张开后重新武装。
`torque_guard_action="release"` 恢复旧行为 —— **就地**把刚度和阻尼降到零,工件会直接把
钳口顶开。默认是 `0` —— **不显式要求就不启用**。这个值要按机器挑:它取决于爪子之间那个
工件有多脆。
- **从端的夹持力可以让主端感觉到。** 打开 `force_feedback=True` 后,从端在
`litearm/v4/{grip_id}/gripper_feedback` 上发布 `force_n`(正 = 夹紧,已按安装方向归一化),
主端按 `min(max(0, force_n - feedback_threshold_n) * feedback_gain_nm_per_n,
feedback_max_tau_nm)` 渲染反向阻力。默认阈值 `2.0` N、增益 `0.10` Nm/N、封顶 `1.0` Nm;
阻尼(`0.1`)只在收到新鲜力时生效,超过 `feedback_timeout_s`(`0.3`)后力矩和阻尼一起归零。
该功能默认关闭,且依赖 zenoh 链路 —— UDP 主端只能发不能收,所以自动没有反馈。新增的 key
是附加的,旧版本对端会自动退化为纯位置遥操。
- **跟丢主端的从端保持原位,不会卸力。** 超过 `watchdog_s`(默认 `0.2`)没有新帧,`stale`
置真、爪子停在原处。
- **从端把主端的速度前馈进去**(对相邻两帧做差分),所以不会拖在移动的主端后面。`dq_max=0`
Expand Down
22 changes: 22 additions & 0 deletions src/litegrip/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -144,14 +144,25 @@ def _detect_version(dist_name: str = "litegrip-python") -> str:
DEFAULT_READY_TOLERANCE_MM,
DEFAULT_READY_PERIOD_S,
DEFAULT_READY_TIMEOUT_S,
DEFAULT_FEEDBACK_GAIN_NM_PER_N,
DEFAULT_FEEDBACK_THRESHOLD_N,
DEFAULT_FEEDBACK_MAX_TAU_NM,
DEFAULT_FEEDBACK_DAMPING,
DEFAULT_FEEDBACK_TIMEOUT_S,
TORQUE_GUARD_HOLD,
TORQUE_GUARD_RELEASE,
FRAME_SIZE,
READY_FRAME_SIZE,
FEEDBACK_FRAME_SIZE,
encode_frame,
decode_frame,
encode_ready_frame,
decode_ready_frame,
encode_feedback_frame,
decode_feedback_frame,
teleop_topic,
ready_topic,
feedback_topic,
)

# ── Trajectory record and replay ────────────────────────────────────────
Expand Down Expand Up @@ -268,14 +279,25 @@ def __dir__():
"DEFAULT_READY_TOLERANCE_MM",
"DEFAULT_READY_PERIOD_S",
"DEFAULT_READY_TIMEOUT_S",
"DEFAULT_FEEDBACK_GAIN_NM_PER_N",
"DEFAULT_FEEDBACK_THRESHOLD_N",
"DEFAULT_FEEDBACK_MAX_TAU_NM",
"DEFAULT_FEEDBACK_DAMPING",
"DEFAULT_FEEDBACK_TIMEOUT_S",
"TORQUE_GUARD_HOLD",
"TORQUE_GUARD_RELEASE",
"FRAME_SIZE",
"READY_FRAME_SIZE",
"FEEDBACK_FRAME_SIZE",
"encode_frame",
"decode_frame",
"encode_ready_frame",
"decode_ready_frame",
"encode_feedback_frame",
"decode_feedback_frame",
"teleop_topic",
"ready_topic",
"feedback_topic",
# Teleoperation — zenoh link (resolved lazily; needs litegrip-python[zenoh])
"ZenohTeleopTransport",
"Listener",
Expand Down
67 changes: 55 additions & 12 deletions src/litegrip/gripper.py
Original file line number Diff line number Diff line change
Expand Up @@ -47,14 +47,19 @@
_check_calibrated,
clamp_to_travel,
)
from .teleop import (DEFAULT_ALIGN_SPEED_MM_S, DEFAULT_DQ_MAX, DEFAULT_GRIP_ID,
from .teleop import (DEFAULT_ALIGN_SPEED_MM_S, DEFAULT_DQ_MAX,
DEFAULT_FEEDBACK_DAMPING, DEFAULT_FEEDBACK_GAIN_NM_PER_N,
DEFAULT_FEEDBACK_MAX_TAU_NM, DEFAULT_FEEDBACK_THRESHOLD_N,
DEFAULT_FEEDBACK_TIMEOUT_S, DEFAULT_GRIP_ID,
DEFAULT_GRIP_PORT, DEFAULT_LEAD_CAP_MM,
DEFAULT_READY_PERIOD_S, DEFAULT_READY_TIMEOUT_S,
DEFAULT_READY_TOLERANCE_MM, DEFAULT_TORQUE_LIMIT_NM)
DEFAULT_READY_TOLERANCE_MM, DEFAULT_TORQUE_LIMIT_NM,
TORQUE_GUARD_HOLD)


def _zenoh_transport(role: str, key: str, port: int, host: Optional[str],
ready_key: Optional[str] = None) -> "TeleopTransport":
ready_key: Optional[str] = None,
feedback_key: Optional[str] = None) -> "TeleopTransport":
"""Build a point-to-point zenoh transport, or explain how to get one.

zenoh is an optional dependency, so a bare ``import`` failure is turned into
Expand All @@ -68,7 +73,7 @@ def _zenoh_transport(role: str, key: str, port: int, host: Optional[str],
"the zenoh teleoperation link needs the optional zenoh dependency — "
"install it with `pip install litegrip-python[zenoh]`") from e
return ZenohTeleopTransport(role, key, port=port, host=host,
ready_key=ready_key)
ready_key=ready_key, feedback_key=feedback_key)


# The calibration the SDK ships for the unit it ships with. This is the
Expand Down Expand Up @@ -2193,11 +2198,18 @@ def teleop_start(
watchdog_s: float = 0.2,
dq_max: float = DEFAULT_DQ_MAX,
torque_limit_nm: float = DEFAULT_TORQUE_LIMIT_NM,
torque_guard_action: str = TORQUE_GUARD_HOLD,
lead_cap_mm: float = DEFAULT_LEAD_CAP_MM, # [遥操对齐块]
require_ready: bool = True,
ready_timeout_s: float = DEFAULT_READY_TIMEOUT_S,
ready_tolerance_mm: float = DEFAULT_READY_TOLERANCE_MM,
ready_period_s: float = DEFAULT_READY_PERIOD_S,
force_feedback: bool = False,
feedback_gain_nm_per_n: float = DEFAULT_FEEDBACK_GAIN_NM_PER_N,
feedback_threshold_n: float = DEFAULT_FEEDBACK_THRESHOLD_N,
feedback_max_tau_nm: float = DEFAULT_FEEDBACK_MAX_TAU_NM,
feedback_damping: float = DEFAULT_FEEDBACK_DAMPING,
feedback_timeout_s: float = DEFAULT_FEEDBACK_TIMEOUT_S,
rate_hz: float = 50.0,
) -> dict:
"""Start leader/follower teleoperation on this gripper.
Expand Down Expand Up @@ -2234,8 +2246,13 @@ def teleop_start(
dq_max: Slave only — ceiling in rad/s on the leader velocity fed
forward to the follower. ``0`` disables the feedforward.
torque_limit_nm: Slave only — ceiling in Nm on the follower's own
torque; held over it the follower releases in place. ``0``
disables the guard. See :class:`~litegrip.GripperTeleop`.
torque; held over it the follower acts per
``torque_guard_action``. ``0`` disables the guard. See
:class:`~litegrip.GripperTeleop`.
torque_guard_action: Slave only — ``"hold"`` (default) keeps
pressing with a bounded pure-torque frame so a grasp survives;
``"release"`` drops to zero stiffness and lets go, the previous
behaviour.
lead_cap_mm: Slave only — ceiling in mm on how far the *align*
command may lead the measured position, which bounds the align
torque. The follow loop is not capped. ``0`` disables the cap.
Expand All @@ -2249,6 +2266,17 @@ def teleop_start(
ready_tolerance_mm: Slave only — arrival tolerance for announcing
ready, in mm of travel.
ready_period_s: Slave only — how often to republish the ready state.
force_feedback: Send the follower's squeeze back and render it on
the leader as resistance. Needs the zenoh link (the UDP master
is publish-only and silently runs without it).
feedback_gain_nm_per_n: Leader torque per newton of follower
squeeze above the threshold. ``0`` disables it.
feedback_threshold_n: Follower squeeze below which the leader feels
nothing.
feedback_max_tau_nm: Ceiling on the leader's rendered resistance.
feedback_damping: Leader damping applied only during contact.
feedback_timeout_s: Age past which the force is ignored and both
torque and damping drop to zero.
rate_hz: Loop rate.

Returns:
Expand All @@ -2264,7 +2292,7 @@ def teleop_start(
positive.
"""
from .teleop import (GripperTeleop, TeleopBusyError, check_ready,
ready_topic, teleop_topic)
feedback_topic, ready_topic, teleop_topic)
from .teleop import UdpTeleopTransport

self._check_connected()
Expand All @@ -2286,6 +2314,11 @@ def teleop_start(
try:
key = teleop_topic(grip_id)
ready_key = ready_topic(grip_id)
# Declared on every session, even with the feature off: the leader's
# zenoh endpoint is resident and its key set cannot change later.
# An unused key costs nothing and a later session that enables
# feedback then just works.
feedback_key = feedback_topic(grip_id)
created_transport = None
if transport is None:
if link == "zenoh":
Expand All @@ -2295,10 +2328,11 @@ def teleop_start(
# 已死的端点 ⇒ 此后每一轮都在往死会话里 put。(真机实测:
# 第一次配对正常,之后每次从端 0 帧。)常驻端点的生命周期
# 只归 `disconnect()` → `_close_teleop_pub()`。
transport = self._open_teleop_pub(port, key, ready_key)
transport = self._open_teleop_pub(port, key, ready_key,
feedback_key)
else:
transport = _zenoh_transport("slave", key, port, host,
ready_key)
ready_key, feedback_key)
created_transport = transport
elif link == "udp":
if host is None:
Expand All @@ -2324,10 +2358,18 @@ def teleop_start(
align_speed_mm_s=align_speed_mm_s, # [遥操对齐块]
watchdog_s=watchdog_s, dq_max=dq_max,
torque_limit_nm=torque_limit_nm, lead_cap_mm=lead_cap_mm, # [遥操对齐块]
torque_guard_action=torque_guard_action,
ready_topic=ready_key, require_ready=require_ready,
ready_timeout_s=ready_timeout_s,
ready_tolerance_mm=ready_tolerance_mm,
ready_period_s=ready_period_s)
ready_period_s=ready_period_s,
force_feedback=force_feedback,
feedback_topic=feedback_key,
feedback_gain_nm_per_n=feedback_gain_nm_per_n,
feedback_threshold_n=feedback_threshold_n,
feedback_max_tau_nm=feedback_max_tau_nm,
feedback_damping=feedback_damping,
feedback_timeout_s=feedback_timeout_s)
manager.start()
except BaseException:
self._release_session("teleop")
Expand All @@ -2337,7 +2379,8 @@ def teleop_start(
return manager.status()

def _open_teleop_pub(self, port: int, key: str,
ready_key: Optional[str] = None) -> "TeleopTransport":
ready_key: Optional[str] = None,
feedback_key: Optional[str] = None) -> "TeleopTransport":
"""Return the leader's resident publisher, building it on first use.

⚠ **Resident, not per session.** Rebuilding the zenoh listener on every
Expand All @@ -2349,7 +2392,7 @@ def _open_teleop_pub(self, port: int, key: str,
"""
if self._teleop_pub is None:
self._teleop_pub = _zenoh_transport("master", key, port, None,
ready_key)
ready_key, feedback_key)
return self._teleop_pub

def _close_teleop_pub(self) -> None:
Expand Down
Loading
Loading