@@ -86,7 +87,7 @@
Control session
Robot workspace
- Tool targetSimulated toolPath
diff --git a/src/source_robotics_leap_motion/lab.js b/src/source_robotics_leap_motion/lab.js
index 2285c68..f0d708f 100644
--- a/src/source_robotics_leap_motion/lab.js
+++ b/src/source_robotics_leap_motion/lab.js
@@ -107,6 +107,9 @@ function update() {
$("connection").textContent = "Local session";
$("connection-dot").className = "dot on";
$("source").textContent = d.source;
+ $("receiver-status").textContent = d.receiver
+ ? `Interpolated receiver: ${d.receiver.state} · ${d.receiver.accepted_commands} commands`
+ : "Immediate simulated feedback";
$("hand-label").textContent = d.hand
? `${d.handedness} hand · #${d.hand.id}`
: `No ${d.handedness} hand visible`;
diff --git a/src/source_robotics_leap_motion/lab.py b/src/source_robotics_leap_motion/lab.py
index 6f09cee..82fe559 100644
--- a/src/source_robotics_leap_motion/lab.py
+++ b/src/source_robotics_leap_motion/lab.py
@@ -1,4 +1,4 @@
-"""Loopback-only browser lab. Its only output adapter is SimulatedArm."""
+"""Loopback-only browser lab with exclusively simulated output adapters."""
from __future__ import annotations
@@ -15,6 +15,7 @@
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
from pathlib import Path
+from .adapters.receiver import SimulatedReceiver
from .adapters.simulated import SimulatedArm
from .backends.replay import ReplayBackend
from .control_config import GripperMapping, TeleoperationConfig, Workspace
@@ -22,6 +23,7 @@
from .errors import TeleoperationError, TrackingTimeoutError
from .mapping import Mapping
from .models import Vector3
+from .receiver import ReceiverConfig
from .teleoperation import Teleoperator
from .tracker import Tracker
@@ -154,7 +156,9 @@ def __exit__(self, *_exc):
self._thread.join(2)
-def snapshot(teleop: Teleoperator, robot: SimulatedArm, source: str, message: str) -> dict:
+def snapshot(
+ teleop: Teleoperator, robot: SimulatedArm | SimulatedReceiver, source: str, message: str
+) -> dict:
status = teleop.status
hand = (
None
@@ -178,6 +182,7 @@ def snapshot(teleop: Teleoperator, robot: SimulatedArm, source: str, message: st
"handedness": teleop.config.handedness,
"hand": None if hand is None else {"id": hand.id, "position": asdict(hand.palm.position)},
"robot": asdict(robot.read_feedback()),
+ "receiver": asdict(robot.receiver_status) if isinstance(robot, SimulatedReceiver) else None,
"control": asdict(status),
"tracking": teleop.tracker.health.to_dict(),
"config": asdict(teleop.config),
@@ -213,7 +218,21 @@ def run_lab(args) -> int:
)
source = "Leap controller"
center = (config.workspace.minimum + config.workspace.maximum) * 0.5
- robot = SimulatedArm(Pose(center))
+ robot = (
+ SimulatedReceiver(
+ Pose(center),
+ config=ReceiverConfig(
+ workspace=config.workspace,
+ max_linear_speed=config.max_linear_speed,
+ max_angular_speed=config.max_angular_speed,
+ max_gripper_speed=1.0 if config.gripper is None else config.gripper.max_speed,
+ interpolation_time=min(0.04, config.tracking_timeout),
+ command_timeout=config.tracking_timeout,
+ ),
+ )
+ if args.interpolate
+ else SimulatedArm(Pose(center))
+ )
server = None if args.headless else LabServer(args.port)
auto_pending = args.auto_arm
processed = 0
diff --git a/src/source_robotics_leap_motion/receiver.py b/src/source_robotics_leap_motion/receiver.py
new file mode 100644
index 0000000..14da532
--- /dev/null
+++ b/src/source_robotics_leap_motion/receiver.py
@@ -0,0 +1,285 @@
+"""Deterministic Cartesian receiver with local expiry and bounded interpolation.
+
+No transport, robot SDK, thread, or source-clock synchronization is involved.
+Call advance from a receiver-owned loop independently of the sending application.
+"""
+
+from __future__ import annotations
+
+import math
+import time
+from dataclasses import dataclass
+from typing import Literal
+
+from ._geometry import angle, slerp
+from .control_config import Workspace, _number
+from .control_models import Pose, PoseTarget, RobotFeedback
+from .models import Quaternion, Vector3
+
+
+@dataclass(frozen=True, slots=True)
+class ReceiverConfig:
+ workspace: Workspace
+ max_linear_speed: float
+ max_angular_speed: float = 1.0
+ max_gripper_speed: float = 1.0
+ interpolation_time: float = 0.04
+ command_timeout: float = 0.15
+
+ def __post_init__(self) -> None:
+ if not isinstance(self.workspace, Workspace):
+ raise ValueError("Receiver workspace must be a Workspace")
+ for name in (
+ "max_linear_speed",
+ "max_angular_speed",
+ "max_gripper_speed",
+ "command_timeout",
+ ):
+ _number(getattr(self, name), "Receiver " + name)
+ _number(self.interpolation_time, "Receiver interpolation_time", zero=True)
+ if self.interpolation_time > self.command_timeout:
+ raise ValueError("Receiver interpolation_time must not exceed command_timeout")
+
+
+@dataclass(frozen=True, slots=True)
+class ReceiverStatus:
+ state: Literal["disarmed", "armed", "active", "held", "expired", "fault", "closed"]
+ reason: str
+ session_id: str | None
+ sequence: int
+ accepted_commands: int
+ rejected_commands: int
+ last_rejection: str | None
+ last_receipt_age_s: float | None
+
+
+@dataclass(frozen=True, slots=True)
+class _Ramp:
+ start: Vector3 | Quaternion | float
+ goal: Vector3 | Quaternion | float
+ started: float
+ duration: float
+ expires: float
+
+ def sample(self, now: float):
+ elapsed = max(0.0, min(now, self.expires) - self.started)
+ fraction = 1.0 if self.duration == 0 else min(1.0, elapsed / self.duration)
+ if fraction >= 1:
+ return self.goal
+ if isinstance(self.start, Vector3):
+ return self.start + (self.goal - self.start) * fraction
+ if isinstance(self.start, Quaternion):
+ return slerp(self.start, self.goal, fraction)
+ return self.start + (self.goal - self.start) * fraction
+
+
+class CartesianReceiver:
+ """Keep one plan per channel and never replay a FIFO of stale targets.
+
+ Explicit begin_session is required before accepting commands and after expiry.
+ Session IDs must be unique per new calibration. A held clutch can resume the
+ same session, retaining its sequence watermark; other holds require a new ID.
+ Source timestamps are metadata only. Every deadline uses local receipt time.
+ """
+
+ def __init__(self, initial: RobotFeedback, *, config: ReceiverConfig) -> None:
+ if not isinstance(initial, RobotFeedback):
+ raise ValueError("Supply initial RobotFeedback")
+ if not config.workspace.contains(initial.pose.position):
+ raise ValueError("Initial receiver pose is outside its workspace")
+ self.config = config
+ self._feedback = initial
+ self._state = "disarmed"
+ self._reason = "not_armed"
+ self._session: str | None = None
+ self._sequence = self._accepted = self._rejected = 0
+ self._rejection: str | None = None
+ self._last_now: float | None = None
+ self._received: float | None = None
+ self._deadline: float | None = None
+ self._resume_allowed = False
+ self._position: _Ramp | None = None
+ self._orientation: _Ramp | None = None
+ self._gripper: _Ramp | None = None
+
+ @property
+ def feedback(self) -> RobotFeedback:
+ return self._feedback
+
+ @property
+ def status(self) -> ReceiverStatus:
+ age = None if self._received is None else self._last_now - self._received
+ return ReceiverStatus(
+ self._state,
+ self._reason,
+ self._session,
+ self._sequence,
+ self._accepted,
+ self._rejected,
+ self._rejection,
+ age,
+ )
+
+ def _now(self, now: float | None) -> float:
+ value = time.monotonic() if now is None else now
+ if (
+ isinstance(value, bool)
+ or not isinstance(value, (int, float))
+ or not math.isfinite(value)
+ or value < 0
+ ):
+ self.fault("invalid_receiver_time")
+ raise ValueError("Receiver time must be finite and non-negative")
+ if self._last_now is not None and value < self._last_now:
+ self.fault("receiver_clock_regressed")
+ raise ValueError("Receiver time moved backwards")
+ self._last_now = value
+ return value
+
+ def _cancel(self) -> None:
+ self._position = self._orientation = self._gripper = None
+ self._deadline = None
+
+ def fault(self, reason: str) -> None:
+ self._cancel()
+ self._resume_allowed = False
+ self._state, self._reason = "fault", reason
+
+ def advance(self, *, now: float | None = None) -> RobotFeedback:
+ now = self._now(now)
+ if self._state not in ("armed", "active"):
+ return self._feedback
+ effective = min(now, self._deadline)
+ pose = self._feedback.pose
+ position = pose.position if self._position is None else self._position.sample(effective)
+ orientation = (
+ pose.orientation if self._orientation is None else self._orientation.sample(effective)
+ )
+ gripper = (
+ self._feedback.gripper if self._gripper is None else self._gripper.sample(effective)
+ )
+ self._feedback = RobotFeedback(Pose(position, orientation), gripper)
+ if now >= self._deadline:
+ self._cancel()
+ self._resume_allowed = False
+ self._state, self._reason = "expired", "command_expired"
+ return self._feedback
+
+ def begin_session(
+ self, session_id: str, *, after_sequence: int | None = None, now: float | None = None
+ ) -> None:
+ self.advance(now=now)
+ if not isinstance(session_id, str) or not session_id or len(session_id) > 128:
+ raise ValueError("Session ID must be a non-empty string of at most 128 characters")
+ if self._state in ("fault", "closed"):
+ raise ValueError("Reopen the receiver after fault or close")
+ same = session_id == self._session
+ if same and not self._resume_allowed:
+ raise ValueError("Use a new session after disarming or command expiry")
+ if after_sequence is None:
+ if same:
+ raise ValueError("Resume requires the sender's sequence watermark")
+ after_sequence = 0
+ if type(after_sequence) is not int or after_sequence < 0:
+ raise ValueError("Session sequence watermark must be a non-negative integer")
+ if self._state in ("armed", "active"):
+ raise ValueError("Hold the current receiver session before starting another")
+ if same and after_sequence < self._sequence:
+ raise ValueError("Session sequence watermark must not move backwards")
+ self._cancel()
+ self._sequence = after_sequence
+ self._session = session_id
+ self._received = None
+ self._resume_allowed = False
+ self._deadline = self._last_now + self.config.command_timeout
+ self._state, self._reason = "armed", "waiting_for_command"
+
+ def _reject(self, reason: str) -> bool:
+ self._rejected += 1
+ self._rejection = reason
+ return False
+
+ def accept(self, target: PoseTarget, *, now: float | None = None) -> bool:
+ self.advance(now=now)
+ if self._state not in ("armed", "active"):
+ return self._reject("receiver_not_armed")
+ if not isinstance(target, PoseTarget):
+ return self._reject("invalid_target")
+ if target.session_id != self._session:
+ return self._reject("session_mismatch")
+ if type(target.sequence) is not int or target.sequence <= self._sequence:
+ return self._reject("sequence_not_increasing")
+ if (
+ not isinstance(target.position, Vector3)
+ or (target.orientation is not None and not isinstance(target.orientation, Quaternion))
+ or (
+ target.gripper is not None
+ and (
+ isinstance(target.gripper, bool)
+ or not isinstance(target.gripper, (int, float))
+ or not math.isfinite(target.gripper)
+ or not 0 <= target.gripper <= 1
+ )
+ )
+ or isinstance(target.valid_for_s, bool)
+ or not isinstance(target.valid_for_s, (int, float))
+ or not math.isfinite(target.valid_for_s)
+ or target.valid_for_s <= 0
+ or type(target.source_frame_id) is not int
+ or target.source_frame_id < 0
+ or type(target.source_timestamp_us) is not int
+ or target.source_timestamp_us < 0
+ or not isinstance(target.clock_domain, str)
+ or not target.clock_domain
+ ):
+ return self._reject("invalid_target")
+ if not self.config.workspace.contains(target.position):
+ return self._reject("outside_workspace")
+ if target.gripper is not None and self._feedback.gripper is None:
+ return self._reject("missing_gripper_reference")
+ now = self._last_now
+ deadline = now + min(target.valid_for_s, self.config.command_timeout)
+ current = self._feedback
+ duration = max(
+ self.config.interpolation_time,
+ (target.position - current.pose.position).norm / self.config.max_linear_speed,
+ )
+ self._position = _Ramp(current.pose.position, target.position, now, duration, deadline)
+ if target.orientation is not None:
+ duration = max(
+ self.config.interpolation_time,
+ angle(current.pose.orientation, target.orientation) / self.config.max_angular_speed,
+ )
+ self._orientation = _Ramp(
+ current.pose.orientation, target.orientation, now, duration, deadline
+ )
+ if target.gripper is not None:
+ duration = max(
+ self.config.interpolation_time,
+ abs(target.gripper - current.gripper) / self.config.max_gripper_speed,
+ )
+ self._gripper = _Ramp(current.gripper, target.gripper, now, duration, deadline)
+ self._received, self._deadline = now, deadline
+ self._sequence = target.sequence
+ self._accepted += 1
+ self._rejection = None
+ self._state, self._reason = "active", "interpolating"
+ return True
+
+ def hold(self, reason: str = "held", *, now: float | None = None) -> None:
+ self.advance(now=now)
+ if self._state in ("fault", "closed"):
+ return
+ self._resume_allowed = reason == "clutched" and (
+ self._state in ("armed", "active") or self._resume_allowed
+ )
+ self._cancel()
+ self._state, self._reason = "held", reason
+
+ def close(self, *, now: float | None = None) -> None:
+ try:
+ self.advance(now=now)
+ finally:
+ self._cancel()
+ self._resume_allowed = False
+ self._state, self._reason = "closed", "closed"
diff --git a/src/source_robotics_leap_motion/teleoperation.py b/src/source_robotics_leap_motion/teleoperation.py
index 72f2422..1d24b24 100644
--- a/src/source_robotics_leap_motion/teleoperation.py
+++ b/src/source_robotics_leap_motion/teleoperation.py
@@ -55,6 +55,10 @@ def __enter__(self) -> Self:
raise TeleoperationError("Adapter does not support configured orientation control")
if self.config.gripper is not None and not capabilities.gripper:
raise TeleoperationError("Adapter does not support configured gripper control")
+ if capabilities.command_sessions and not callable(
+ getattr(self.robot, "begin_session", None)
+ ):
+ raise TeleoperationError("Session-aware adapter must implement begin_session")
self.controller = TeleoperationController(self.config)
self._disconnected = False
self.last_frame = None
@@ -80,7 +84,7 @@ def _call(self, operation: str, *arguments):
except Exception as error:
self.controller.fault(f"adapter_{operation}_failed")
extra = ""
- if operation in ("send", "read_feedback"):
+ if operation in ("send", "read_feedback", "begin_session"):
try:
self.robot.hold("adapter_fault")
except Exception as hold_error:
@@ -109,6 +113,8 @@ def arm(self, reference: RobotFeedback | None = None, *, timeout: float | None =
if health.status != "streaming":
raise TeleoperationError("Calibration requires tracking that is currently streaming")
self.controller.arm(frame, reference, frame_age=health.last_frame_age_s or 0.0)
+ if self.robot.capabilities.command_sessions:
+ self._call("begin_session", self.status.session_id, self.status.sequence)
def disarm(self) -> None:
self._require_open()
@@ -167,6 +173,9 @@ def step(self, *, timeout: float | None = None) -> PoseTarget | None:
raise
if before in ("active", "clutched") and self.status.state == "tracking_lost":
self._call("hold", self.status.reason)
+ if before == "clutched" and self.status.state == "active":
+ if self.robot.capabilities.command_sessions:
+ self._call("begin_session", self.status.session_id, self.status.sequence)
if target is not None:
self._call("send", target)
return target
diff --git a/tests/test_control.py b/tests/test_control.py
index b066a11..ac74d36 100644
--- a/tests/test_control.py
+++ b/tests/test_control.py
@@ -223,6 +223,58 @@ def test_gripper_mapping_limits_speed_and_holds_during_clutch():
assert controller.status.last_target.gripper == pytest.approx(0.95)
+def test_gripper_smoothing_is_independent_of_position_smoothing():
+ controller = armed(
+ smoothing_time_constant=0,
+ gripper=GripperMapping(smoothing_time_constant=1, max_speed=100, deadband=0),
+ )
+ target = controller.update(frame(1, Vector3(0.1, 0, 0), grab=1), now=0.1)
+ assert target.position.x == pytest.approx(0.1)
+ assert target.gripper == pytest.approx(math.exp(-0.1))
+
+
+def test_gripper_defaults_inherit_pose_smoothing_and_emit_on_each_pose_command():
+ controller = armed(
+ smoothing_time_constant=1,
+ gripper=GripperMapping(max_speed=100, deadband=0),
+ )
+ target = controller.update(frame(1, Vector3(1, 0, 0), grab=1), now=0.1)
+ assert target.position.x == pytest.approx(1 - math.exp(-0.1))
+ assert target.gripper == pytest.approx(math.exp(-0.1))
+
+
+def test_slower_gripper_rate_does_not_slow_pose_updates_or_bypass_gripper_speed_limits():
+ controller = armed(
+ max_output_hz=100,
+ gripper=GripperMapping(max_output_hz=20, max_speed=0.5, deadband=0),
+ )
+ outputs = []
+ for index in range(1, 11):
+ target = controller.update(
+ frame(index, Vector3(index / 100, 0, 0), grab=1, timestamp=index * 10_000),
+ now=index / 100,
+ )
+ assert target is not None
+ if target.gripper is not None:
+ outputs.append((index, target.gripper))
+ assert outputs == [(5, pytest.approx(0.975)), (10, pytest.approx(0.95))]
+ assert controller.status.sequence == 10
+
+
+def test_clutch_reanchor_resets_gripper_timing_and_reference():
+ controller = armed(
+ tracking_timeout=2,
+ gripper=GripperMapping(max_output_hz=20, max_speed=0.5, deadband=0),
+ )
+ controller.update(frame(1, grab=1), now=0.1)
+ controller.clutch()
+ controller.update(frame(2), now=0.2)
+ controller.release_clutch(RobotFeedback(Pose(Vector3()), 0.5))
+ assert controller.update(frame(3), now=0.7) is None
+ assert controller.update(frame(4), now=0.71).gripper is None
+ assert controller.update(frame(5), now=0.75).gripper == pytest.approx(0.525)
+
+
def test_output_frequency_is_a_cap_and_skipped_inputs_still_refresh_tracking():
controller = armed(max_output_hz=10)
for index in range(1, 10):
diff --git a/tests/test_control_config.py b/tests/test_control_config.py
index 7d6fc29..c61ccc6 100644
--- a/tests/test_control_config.py
+++ b/tests/test_control_config.py
@@ -31,7 +31,17 @@ def test_workspace_rejects_inverted_bounds():
@pytest.mark.parametrize(
"settings",
- [{"source": "fist"}, {"max_speed": 0}, {"open_value": 1.1}, {"closed_value": float("nan")}],
+ [
+ {"source": "fist"},
+ {"max_speed": 0},
+ {"open_value": 1.1},
+ {"closed_value": float("nan")},
+ {"smoothing_time_constant": -1},
+ {"smoothing_time_constant": True},
+ {"max_output_hz": 0},
+ {"max_output_hz": float("inf")},
+ {"max_output_hz": 1001},
+ ],
)
def test_invalid_gripper_settings(settings):
with pytest.raises(ValueError):
@@ -51,12 +61,28 @@ def test_toml_configuration_and_typo_rejection(tmp_path):
control_orientation = true
[gripper]
source = "pinch"
+smoothing_time_constant = 0.2
+max_output_hz = 20
"""
path.write_text(text, encoding="utf-8")
config = TeleoperationConfig.from_toml(path)
assert config.mapping.axes == ("-z", "-x", "y")
assert config.gripper.source == "pinch"
assert config.control_orientation
+ assert config.gripper.smoothing_time_constant == 0.2
+ assert config.gripper.max_output_hz == 20
path.write_text(text.replace("max_linear_speed", "max_linear_speeed"), encoding="utf-8")
with pytest.raises(ValueError, match="max_linear_speed"):
TeleoperationConfig.from_toml(path)
+
+
+def test_gripper_frequency_cannot_exceed_the_total_command_cap():
+ from source_robotics_leap_motion import GripperMapping
+
+ with pytest.raises(ValueError, match="teleoperation.max_output_hz"):
+ TeleoperationConfig(
+ Workspace(Vector3(), Vector3(1, 1, 1)),
+ 1,
+ max_output_hz=30,
+ gripper=GripperMapping(max_output_hz=60),
+ )
diff --git a/tests/test_lab.py b/tests/test_lab.py
index 7c6af7a..20b7f96 100644
--- a/tests/test_lab.py
+++ b/tests/test_lab.py
@@ -27,6 +27,31 @@ def test_headless_demo_can_explicitly_arm_simulation(capsys):
assert states[-1]["control"]["sequence"] > 0
+def test_headless_demo_can_use_the_interpolating_receiver_and_closes_its_worker(capsys):
+ import threading
+
+ assert (
+ main(
+ [
+ "teleop",
+ "--demo",
+ "--interpolate",
+ "--headless",
+ "--auto-arm",
+ "--json",
+ "--frames",
+ "3",
+ ]
+ )
+ == 0
+ )
+ states = [json.loads(line) for line in capsys.readouterr().out.splitlines()]
+ assert len(states) == 3
+ assert states[-1]["receiver"]["accepted_commands"] > 0
+ assert states[-1]["receiver"]["session_id"] == states[-1]["control"]["session_id"]
+ assert not any(t.name == "source-leap-receiver" for t in threading.enumerate())
+
+
def test_lab_server_serves_packaged_viewer_and_authenticated_bounded_actions():
server = LabServer()
with server:
diff --git a/tests/test_receiver.py b/tests/test_receiver.py
new file mode 100644
index 0000000..20c90ff
--- /dev/null
+++ b/tests/test_receiver.py
@@ -0,0 +1,250 @@
+import math
+from dataclasses import replace
+
+import pytest
+
+from source_robotics_leap_motion import (
+ CartesianReceiver,
+ Pose,
+ PoseTarget,
+ Quaternion,
+ ReceiverConfig,
+ RobotFeedback,
+ Vector3,
+ Workspace,
+)
+from source_robotics_leap_motion._geometry import angle
+
+
+def config(**overrides):
+ values = dict(
+ workspace=Workspace(Vector3(-2, -2, -2), Vector3(2, 2, 2)),
+ max_linear_speed=1,
+ max_angular_speed=1,
+ max_gripper_speed=1,
+ interpolation_time=0.1,
+ command_timeout=0.5,
+ )
+ values.update(overrides)
+ return ReceiverConfig(**values)
+
+
+def receiver(**overrides):
+ return CartesianReceiver(RobotFeedback(Pose(Vector3()), 1.0), config=config(**overrides))
+
+
+def target(sequence=1, *, position=None, session="one", ttl=0.5, orientation=None, gripper=None):
+ position = Vector3(1, 0, 0) if position is None else position
+ return PoseTarget(position, orientation, gripper, session, sequence, 10, 1_000, "camera", ttl)
+
+
+def test_session_handshake_is_explicit_and_commands_never_arm_a_receiver():
+ rx = receiver()
+ assert not rx.accept(target(), now=0)
+ assert rx.feedback.pose.position == Vector3()
+ rx.begin_session("one", now=0)
+ assert rx.status.state == "armed"
+ assert not rx.accept(target(session="other"), now=0)
+ assert rx.accept(target(), now=0)
+ assert rx.status.accepted_commands == 1
+
+
+def test_interpolation_is_continuous_when_a_new_target_replaces_the_old_one():
+ rx = receiver(max_linear_speed=10)
+ rx.begin_session("one", now=0)
+ rx.accept(target(), now=0)
+ before = rx.advance(now=0.04)
+ assert before.pose.position.x == pytest.approx(0.4)
+ assert rx.accept(target(2, position=Vector3(-0.5, 0, 0)), now=0.04)
+ assert rx.feedback == before
+ assert rx.advance(now=0.09).pose.position.x == pytest.approx(-0.05)
+ assert before.pose.position.x == pytest.approx(0.4) # Owned feedback remains unchanged.
+
+
+def test_delayed_tick_stops_at_the_expiry_instant_not_the_eventual_tick_time():
+ rx = receiver(max_linear_speed=0.2)
+ rx.begin_session("one", now=0)
+ rx.accept(target(ttl=0.1), now=0)
+ result = rx.advance(now=10)
+ assert result.pose.position.x == pytest.approx(0.02)
+ assert rx.status.state == "expired"
+ assert not rx.accept(target(2), now=10)
+ with pytest.raises(ValueError, match="new session"):
+ rx.begin_session("one", now=10)
+ rx.begin_session("two", now=10)
+ assert not rx.accept(target(3, session="one"), now=10)
+ assert rx.accept(target(1, session="two"), now=10)
+
+
+def test_duplicates_and_out_of_order_commands_do_not_refresh_expiry():
+ rx = receiver()
+ rx.begin_session("one", now=0)
+ rx.accept(target(5, ttl=0.1), now=0)
+ for index, sequence in enumerate((5, 3, 0), start=1):
+ assert not rx.accept(target(sequence), now=index * 0.02)
+ rx.advance(now=0.1)
+ assert rx.status.state == "expired"
+ assert rx.status.sequence == 5
+ assert rx.status.rejected_commands == 3
+
+
+def test_source_clocks_cannot_extend_the_local_timeout_cap():
+ rx = receiver(command_timeout=0.2)
+ rx.begin_session("one", now=5)
+ command = replace(target(ttl=1000), source_timestamp_us=10**20, clock_domain="remote-future")
+ assert rx.accept(command, now=5)
+ assert rx.advance(now=5.2).pose.position.x == pytest.approx(0.2)
+ assert rx.status.state == "expired"
+
+
+def test_no_first_command_also_expires_and_holds_the_initial_pose():
+ rx = receiver(command_timeout=0.2)
+ rx.begin_session("one", now=0)
+ assert rx.advance(now=0.2).pose.position == Vector3()
+ assert rx.status.state == "expired"
+ assert not rx.accept(target(), now=0.2)
+
+
+def test_clutch_holds_and_explicit_resume_keeps_the_sequence_watermark():
+ rx = receiver()
+ rx.begin_session("one", now=0)
+ rx.accept(target(5), now=0)
+ rx.hold("clutched", now=0.03)
+ held = rx.feedback
+ rx.hold("clutched", now=0.04) # Repeating a hold is idempotent.
+ assert rx.advance(now=8) == held
+ assert not rx.accept(target(6), now=8)
+ rx.begin_session("one", after_sequence=5, now=8)
+ assert not rx.accept(target(5), now=8)
+ assert rx.accept(target(6), now=8)
+ rx.hold("hand_missing", now=8.01)
+ with pytest.raises(ValueError, match="new session"):
+ rx.begin_session("one", now=8.01)
+
+
+def test_resume_rejects_unseen_packets_sent_before_clutching():
+ rx = receiver()
+ rx.begin_session("one", now=0)
+ rx.accept(target(5), now=0)
+ rx.hold("clutched", now=0.03)
+ with pytest.raises(ValueError, match="watermark"):
+ rx.begin_session("one", now=0.04)
+ with pytest.raises(ValueError, match="watermark"):
+ rx.begin_session("one", after_sequence=4, now=0.04)
+ # The sender had produced commands through #10, even though only #5 arrived.
+ rx.begin_session("one", after_sequence=10, now=0.04)
+ assert not rx.accept(target(9), now=0.04)
+ assert not rx.accept(target(10), now=0.04)
+ assert rx.accept(target(11), now=0.04)
+
+
+def test_pose_only_updates_preserve_gripper_and_rotation_plans_without_refreshing_them():
+ rx = receiver()
+ rx.begin_session("one", now=0)
+ rotation = Quaternion(0, 0, math.sin(0.5), math.cos(0.5))
+ rx.accept(target(orientation=rotation, gripper=0, ttl=0.1), now=0)
+ rx.accept(target(2, ttl=0.5), now=0.05)
+ assert rx.advance(now=0.075).gripper == pytest.approx(0.925)
+ result = rx.advance(now=0.2)
+ assert rx.status.state == "active"
+ assert result.gripper == pytest.approx(0.9)
+ assert angle(Quaternion(), result.pose.orientation) == pytest.approx(0.1)
+ assert result.pose.position.x == pytest.approx(0.2)
+
+
+def test_each_channel_obeys_its_speed_limit_with_jitter_and_replacement():
+ rx = receiver(max_linear_speed=0.2, max_angular_speed=0.5, max_gripper_speed=0.3)
+ rx.begin_session("one", now=0)
+ rx.accept(target(orientation=Quaternion(0, 0, 1, 0), gripper=0), now=0)
+ previous, previous_time = rx.feedback, 0.0
+ for index in range(1, 81):
+ now = index * 0.005
+ current = rx.advance(now=now)
+ dt = now - previous_time
+ assert (current.pose.position - previous.pose.position).norm <= 0.2 * dt + 1e-12
+ assert angle(previous.pose.orientation, current.pose.orientation) <= 0.5 * dt + 1e-9
+ assert abs(current.gripper - previous.gripper) <= 0.3 * dt + 1e-12
+ assert rx.config.workspace.contains(current.pose.position)
+ if index % 7 == 0:
+ rx.accept(target(index, position=Vector3(-1, 1, 0)), now=now)
+ previous, previous_time = current, now
+
+
+@pytest.mark.parametrize(
+ "changes",
+ [
+ {"sequence": True},
+ {"sequence": 1.5},
+ {"valid_for_s": float("nan")},
+ {"valid_for_s": float("inf")},
+ {"valid_for_s": 0},
+ {"valid_for_s": True},
+ {"position": None},
+ {"orientation": "invalid"},
+ {"gripper": float("nan")},
+ {"gripper": 2},
+ {"gripper": True},
+ {"source_frame_id": -1},
+ {"source_timestamp_us": -1},
+ {"clock_domain": ""},
+ ],
+)
+def test_invalid_packets_do_not_move_or_refresh_the_receiver(changes):
+ rx = receiver()
+ rx.begin_session("one", now=0)
+ assert not rx.accept(replace(target(), **changes), now=0)
+ assert rx.status.accepted_commands == 0
+ assert rx.advance(now=0.5).pose.position == Vector3()
+ assert rx.status.state == "expired"
+
+
+def test_workspace_and_missing_gripper_reference_are_receiver_side_checks():
+ rx = CartesianReceiver(RobotFeedback(Pose(Vector3())), config=config())
+ rx.begin_session("one", now=0)
+ assert not rx.accept(target(position=Vector3(3, 0, 0)), now=0)
+ assert rx.status.last_rejection == "outside_workspace"
+ assert not rx.accept(target(gripper=0), now=0)
+ assert rx.status.last_rejection == "missing_gripper_reference"
+ assert rx.accept(target(), now=0)
+
+
+@pytest.mark.parametrize("now", [-1, float("nan"), float("inf"), True])
+def test_invalid_receiver_clock_latches_a_fault(now):
+ rx = receiver()
+ rx.begin_session("one", now=0)
+ with pytest.raises(ValueError):
+ rx.advance(now=now)
+ assert rx.status.state == "fault"
+ with pytest.raises(ValueError, match="Reopen"):
+ rx.begin_session("two", now=1)
+
+
+def test_regressed_clock_faults_and_close_cancels_all_motion():
+ rx = receiver()
+ rx.begin_session("one", now=1)
+ rx.accept(target(), now=1)
+ feedback = rx.advance(now=1.02)
+ with pytest.raises(ValueError, match="backwards"):
+ rx.advance(now=1.01)
+ assert rx.advance(now=2) == feedback
+ rx.close(now=2)
+ assert rx.advance(now=3) == feedback
+ assert rx.status.state == "closed"
+ with pytest.raises(ValueError, match="Reopen"):
+ rx.begin_session("two", now=3)
+
+
+@pytest.mark.parametrize(
+ "settings",
+ [
+ {"max_linear_speed": 0},
+ {"max_angular_speed": float("nan")},
+ {"max_gripper_speed": True},
+ {"command_timeout": 0},
+ {"interpolation_time": -1},
+ {"interpolation_time": 1},
+ ],
+)
+def test_invalid_receiver_configuration_is_rejected(settings):
+ with pytest.raises(ValueError):
+ config(**settings)
diff --git a/tests/test_receiver_adapter.py b/tests/test_receiver_adapter.py
new file mode 100644
index 0000000..c71f45c
--- /dev/null
+++ b/tests/test_receiver_adapter.py
@@ -0,0 +1,87 @@
+import threading
+import time
+
+import pytest
+
+from source_robotics_leap_motion import (
+ AdapterError,
+ Pose,
+ PoseTarget,
+ ReceiverConfig,
+ Vector3,
+ Workspace,
+)
+from source_robotics_leap_motion.adapters import SimulatedReceiver
+
+
+def robot():
+ return SimulatedReceiver(
+ Pose(Vector3()),
+ config=ReceiverConfig(
+ Workspace(Vector3(-1, -1, -1), Vector3(1, 1, 1)),
+ 1,
+ interpolation_time=0,
+ command_timeout=0.5,
+ ),
+ update_hz=200,
+ )
+
+
+def test_constructor_is_idle_and_worker_expires_while_the_sender_is_not_calling_it():
+ initial_threads = set(threading.enumerate())
+ rx = robot()
+ assert set(threading.enumerate()) == initial_threads
+ rx.open()
+ try:
+ assert rx.worker_alive
+ rx.begin_session("one", 0)
+ rx.send(PoseTarget(Vector3(1, 0, 0), None, None, "one", 1, 1, 1, "camera", 0.03))
+ # Inspect cached status directly: polling must not perform the watchdog work.
+ deadline = time.monotonic() + 2
+ while time.monotonic() < deadline:
+ with rx._lock:
+ expired = rx._receiver.status.state == "expired"
+ feedback = rx._receiver.feedback
+ if expired:
+ break
+ threading.Event().wait(0.005)
+ assert expired
+ assert feedback.pose.position.x == pytest.approx(0.03)
+ with pytest.raises(AdapterError, match="not_armed"):
+ rx.send(PoseTarget(Vector3(), None, None, "one", 2, 2, 2, "camera", 0.1))
+ finally:
+ rx.close()
+ assert not rx.worker_alive
+ assert rx.receiver_status.state == "closed"
+
+
+def test_failed_worker_is_reported_and_even_failed_close_joins_it(monkeypatch):
+ rx = robot()
+ rx.open()
+
+ def fail(*, now=None):
+ raise OSError("receiver clock failed")
+
+ with rx._lock:
+ monkeypatch.setattr(rx._receiver, "advance", fail)
+ assert rx._stop.wait(2)
+ with pytest.raises(AdapterError, match="worker failed"):
+ rx.read_feedback()
+ with pytest.raises(OSError, match="clock failed"):
+ rx.close()
+ assert not rx.worker_alive
+
+
+def test_reopening_starts_a_new_idle_receiver_and_does_not_replay_targets():
+ rx = robot()
+ rx.open()
+ rx.begin_session("one", 0)
+ rx.close()
+ rx.open()
+ try:
+ assert rx.receiver_status.state == "disarmed"
+ with pytest.raises(AdapterError, match="not_armed"):
+ rx.send(PoseTarget(Vector3(), None, None, "one", 1, 1, 1, "camera", 0.1))
+ finally:
+ rx.close()
+ assert not rx.worker_alive
diff --git a/tests/test_teleoperation.py b/tests/test_teleoperation.py
index 0437440..6b94aa7 100644
--- a/tests/test_teleoperation.py
+++ b/tests/test_teleoperation.py
@@ -238,3 +238,67 @@ def fail(reason):
with pytest.raises(AdapterError, match="Hold failed"):
manager.close()
assert not robot._opened
+
+
+def test_receiver_handshakes_follow_explicit_arm_and_completed_clutch_reanchor(setup):
+ source, config, robot = setup
+ sessions = []
+ adapter = CallbackAdapter(
+ robot.send,
+ hold=robot.hold,
+ feedback=robot.read_feedback,
+ open=robot.open,
+ close=robot.close,
+ begin_session=lambda session, sequence: sessions.append((session, sequence)),
+ )
+ with Teleoperator(source, robot=adapter, config=config) as teleop:
+ assert sessions == []
+ teleop.arm()
+ first = teleop.status.session_id
+ assert sessions == [(first, 0)]
+ teleop.step()
+ sent = robot.targets_sent
+ teleop.clutch()
+ teleop.release_clutch()
+ assert sessions == [(first, 0)]
+ assert teleop.step() is None
+ assert sessions == [(first, 0), (first, sent)]
+ assert robot.targets_sent == sent
+ teleop.step()
+ source.events.append(Frame(100, 200_000, ()))
+ teleop.step()
+ assert teleop.status.state == "tracking_lost"
+ teleop.arm()
+ assert len(sessions) == 3 and sessions[-1][0] != first and sessions[-1][1] == 0
+
+
+def test_failed_receiver_handshake_holds_and_latches_adapter_fault(setup):
+ source, config, robot = setup
+
+ def fail(session, sequence):
+ raise OSError("receiver refused session")
+
+ adapter = CallbackAdapter(
+ robot.send,
+ hold=robot.hold,
+ feedback=robot.read_feedback,
+ open=robot.open,
+ close=robot.close,
+ begin_session=fail,
+ )
+ with Teleoperator(source, robot=adapter, config=config) as teleop:
+ with pytest.raises(AdapterError, match="refused session") as error:
+ teleop.arm()
+ assert isinstance(error.value.__cause__, OSError)
+ assert teleop.status.state == "fault"
+ assert robot.hold_reason == "adapter_fault"
+ assert robot.targets_sent == 0
+
+
+def test_missing_session_handshake_is_rejected_before_open(setup):
+ source, config, robot = setup
+ robot.capabilities = RobotCapabilities(feedback=True, command_sessions=True)
+ with pytest.raises(TeleoperationError, match="begin_session"):
+ with Teleoperator(source, robot=robot, config=config):
+ pass
+ assert not robot._opened