diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index f4ca6b0..fa827cf 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -28,6 +28,7 @@ jobs: - run: python -m ruff format --check src tests examples tools/build_runtime_release.py tools/configure_release.py tools/check_release.py tools/probe_private_runtime.py tools/benchmark_latest.py - run: source-leap --demo --frames 3 --json - run: source-leap --demo --latest --health --frames 3 --json + - run: source-leap teleop --demo --headless --auto-arm --frames 3 --json build: runs-on: ubuntu-latest steps: diff --git a/README.md b/README.md index de760c0..495df9c 100644 --- a/README.md +++ b/README.md @@ -4,26 +4,43 @@ A Python package for the **Original Leap Motion Controller**, with owned hand da recording/replay, and configurable coordinate mapping for custom robot integrations. It runs on current Python without `LeapPython.pyd` or a Python 3.8 process. -Beta **0.1.0b2** is published on [PyPI](https://pypi.org/project/source-robotics-leap-motion/). -Install it, then start tracking: +Install the package from [PyPI](https://pypi.org/project/source-robotics-leap-motion/), +then start tracking: ```powershell -python -m pip install source-robotics-leap-motion +python -m pip install --upgrade source-robotics-leap-motion source-leap ``` The Windows engine downloads automatically when needed. First-use driver setup may -require Windows approval. See [release status and validation](docs/BETA_VALIDATION.md) -and [publication steps](docs/RELEASING.md). Linux/macOS currently use an installed +require Windows approval. See [release status and validation](https://github.com/Source-Robotics/Source-Robotics-Leap-Motion/blob/main/docs/BETA_VALIDATION.md) +and [publication steps](https://github.com/Source-Robotics/Source-Robotics-Leap-Motion/blob/main/docs/RELEASING.md). Linux/macOS currently use an installed vendor tracking service and remain experimental for hardware use. -Version **0.1.0b2** includes latest-frame acquisition and tracking health. Both are -available in the published package. See [the latest-frame guide](docs/LATEST_FRAMES.md). +Version **0.1.0b3** adds configurable Cartesian teleoperation, callback integration, +and a browser lab with simulated output. It includes latest-frame acquisition and +tracking health. See +[the teleoperation guide](https://github.com/Source-Robotics/Source-Robotics-Leap-Motion/blob/main/docs/TELEOPERATION.md) and [latest-frame guide](https://github.com/Source-Robotics/Source-Robotics-Leap-Motion/blob/main/docs/LATEST_FRAMES.md). -**Try it in this project's virtual environment.** Python 3.15.2 and the package are -already installed in `.venv`. No activation is necessary: +Try the simulated robot target with a synthetic hand, or with your Leap controller: ```powershell +source-leap teleop --demo +source-leap teleop +``` + +The local browser viewer starts disarmed. Arm to calibrate, clutch to reposition +your hand, and disarm to hold output. Both commands use a simulated Cartesian arm. +The callback API accepts your own controller; joint-only arms supply their kinematics +in the adapter. Configure workspace, speed limits, axes, gains, smoothing, rotation, +and gripper mapping in [teleoperation.toml](https://github.com/Source-Robotics/Source-Robotics-Leap-Motion/blob/main/examples/teleoperation.toml). + +**Run from a source checkout.** Create a virtual environment with Python 3.10 or +newer. No activation is necessary when calling its executables directly: + +```powershell +python -m venv .venv +.\.venv\Scripts\python.exe -m pip install . .\.venv\Scripts\source-leap.exe --demo .\.venv\Scripts\source-leap.exe setup .\.venv\Scripts\source-leap.exe doctor --probe @@ -41,14 +58,20 @@ python -m pip install . source-leap --demo ``` -Python **3.10 or newer** is required. The published b2 wheel was installed and tested on +Python **3.10 or newer** is required. The b3 wheel was installed and tested on **CPython 3.10.11, 3.11.9, 3.12.10, 3.13.17, 3.14.10, and 3.15.2, Windows x64**. -Each version passed all 107 tests, received live camera frames, and replayed a recorded +Each version passed all 180 tests, received live camera frames, and replayed a recorded real hand. Python 3.10 additionally uses small `tomli` and `typing-extensions` backports, installed automatically by pip. All versions use `websocket-client`. Python 3.9 and earlier are not supported by the new package. -The [compatibility and performance report](docs/COMPATIBILITY_PERFORMANCE.md) explains +The live b3 acceptance run checked calibration, clutch/resume, loss of a visible hand, +explicit rearming, disarming, and shutdown against simulated output. Clean-Windows +driver setup and real robot-controller validation remain separate hardware checks. +See the [b3 acceptance evidence](https://github.com/Source-Robotics/Source-Robotics-Leap-Motion/blob/main/docs/B3_VALIDATION.md) and +[clean-machine checklist](https://github.com/Source-Robotics/Source-Robotics-Leap-Motion/blob/main/docs/CLEAN_MACHINE_CHECK.md). + +The [compatibility and performance report](https://github.com/Source-Robotics/Source-Robotics-Leap-Motion/blob/main/docs/COMPATIBILITY_PERFORMANCE.md) explains the test matrix, measured stream rate, comparison with the original Python 3.8 SDK, and remaining limits. The tracking engine and driver are unchanged; no improvement in tracking accuracy or camera frame rate is claimed. @@ -65,7 +88,7 @@ On this PC, the fallback was verified with service `3.2.1+45911` on `127.0.0.1:6437` and the actual Original Leap Motion Controller. A 30-second check captured 3,399 frames, including 53 frames containing a left hand with all five fingers and four bones per finger. Recording and replay also passed. See -[the hardware validation notes](docs/HARDWARE_VALIDATION.md). +[the hardware validation notes](https://github.com/Source-Robotics/Source-Robotics-Leap-Motion/blob/main/docs/HARDWARE_VALIDATION.md). An existing working legacy service can therefore be used without upgrading it. The native LeapC loader supports Windows x64, Linux x64, and macOS Intel/Apple @@ -115,9 +138,9 @@ The project owner confirmed redistribution permission on 5 November 2026. The installed beta wheel also passed two-client sharing, repeated startup, crash cleanup, and a live-frame check of the restored installed service. Clean-PC USB setup and other-platform hardware tests remain pending. -See [the prototype, evidence, and bounded test command](docs/MINIMAL_RUNTIME.md). +See [the prototype, evidence, and bounded test command](https://github.com/Source-Robotics/Source-Robotics-Leap-Motion/blob/main/docs/MINIMAL_RUNTIME.md). The proposed first-use flow and Windows/Linux/macOS targets are documented in -[installation and platforms](docs/INSTALLATION_PLATFORMS.md). +[installation and platforms](https://github.com/Source-Robotics/Source-Robotics-Leap-Motion/blob/main/docs/INSTALLATION_PLATFORMS.md). **Read hand data.** The same API accepts live, synthetic, and replay sources: @@ -164,7 +187,7 @@ The default remains sequential, which is appropriate when recording every frame. received/delivered/skipped/duplicate counts, timeouts, timestamp resets, and hand count. No hands still means the camera is streaming. Age is measured **since Python received the frame**, not end-to-end camera latency. It cannot detect every delay -inside a service or transport. See [semantics and validation](docs/LATEST_FRAMES.md). +inside a service or transport. See [semantics and validation](https://github.com/Source-Robotics/Source-Robotics-Leap-Motion/blob/main/docs/LATEST_FRAMES.md). ```powershell .\.venv\Scripts\source-leap.exe --latest --health --duration 10 @@ -244,6 +267,6 @@ falls back to another backend. Normal users should not need either option. ``` The original scripts and binaries remain historical references. The original README -is preserved in [docs/LEGACY_README.md](docs/LEGACY_README.md). Current scope and design +is preserved in [docs/LEGACY_README.md](https://github.com/Source-Robotics/Source-Robotics-Leap-Motion/blob/main/docs/LEGACY_README.md). Current scope and design are in [PACKAGE_DESIGN.md](PACKAGE_DESIGN.md); native-interface verification is described -in [docs/NATIVE_ABI.md](docs/NATIVE_ABI.md). +in [docs/NATIVE_ABI.md](https://github.com/Source-Robotics/Source-Robotics-Leap-Motion/blob/main/docs/NATIVE_ABI.md). diff --git a/docs/B3_VALIDATION.md b/docs/B3_VALIDATION.md new file mode 100644 index 0000000..1ce587a --- /dev/null +++ b/docs/B3_VALIDATION.md @@ -0,0 +1,55 @@ +**b3 acceptance evidence — 6 November 2026** + +The Original Leap Motion Controller was tested on the development Windows x64 PC, +using CPython 3.15.2 and its existing legacy 3.2.1 service. Control output went to +the simulated Cartesian arm. No physical robot was commanded or driver replaced. + +**Guided live-hand run.** A 34.3-second run processed +2,777 frames, including 2,369 complete hand +samples (five fingers, four bones per finger), and generated 979 +simulated targets. Calibration, clutch hold, re-anchoring without a command on the +re-anchor frame, removal of a visible hand, latched hold after its return, explicit +rearming with a new session, disarming, and adapter/tracker shutdown passed. + +Preliminary runs also held output when a hand briefly disappeared. The successful +run used a stable-hand warmup and a shorter guided cycle to complete each transition. +The test harness changed; the product's conservative loss/rearm policy did not. + +The Python control client used 2.87% of one +core averaged over this mixed hand-present/hand-absent run. Local receipt age at +sampling was 0.085 ms median and +0.205 ms p95. These ages start when Python receives +a distinct frame; they are not exposure-to-command or camera latency measurements. +CPU figures exclude the vendor service and are not a matched comparison with the old SDK. + +**Twenty-minute camera soak.** 1200.0 seconds processed +115,180 frames, including 7,179 complete hand +samples. The camera continued streaming with the hand absent for much of the run. +There were 0 read timeouts. Average delivery was +96.0 FPS; Python-client CPU averaged +2.51% of one core. + +Final working set was 31.66 MiB and lifetime peak +working set was 31.66 MiB. These are Windows +process counters for this Python client, collected with +[GetProcessMemoryInfo](https://learn.microsoft.com/en-us/windows/win32/api/psapi/nf-psapi-getprocessmemoryinfo), +not the vendor engine or total system footprint. History in the runner was bounded. +The tracker closed and left 0 reader threads. + +**Package checks.** The installed b3 code passed 180 tests on each Windows CPython +3.10–3.15 environment, plus all 18 Windows/Linux/macOS CI matrix jobs and the build. +The existing real-hand recording passed simulated control checks on each interpreter. +A fresh Python 3.10 candidate installation passed the headless control demo, packaged +HTML/JavaScript/CSS serving, dependency checks, and thread-free import. Artifact +inspection found no native binaries or personal recordings. Release documentation +updates leave the tested package code, assets, and pinned runtime unchanged. + +**Remaining hardware evidence.** The owner has no spare clean Windows PC available +and chose to continue with the simulator. Clean-PC USB driver setup, actual robot +adapter/controller behavior, and Linux/macOS USB operation remain unverified. +The [clean-machine checklist](CLEAN_MACHINE_CHECK.md) records the test procedure. +Fresh venv and hosted software CI results are not a substitute for those device tests. + +Raw local evidence is retained in `.tools/live-b3-acceptance.json`, its earlier-attempt +reports, `.tools/soak-b3-result.json`, `.tools/b3-candidate-install.json`, +`.tools/b3-artifact-audit.json`, and `.tools/compatibility-results-0.1.0b3.json`. diff --git a/docs/CLEAN_MACHINE_CHECK.md b/docs/CLEAN_MACHINE_CHECK.md new file mode 100644 index 0000000..5e9a757 --- /dev/null +++ b/docs/CLEAN_MACHINE_CHECK.md @@ -0,0 +1,58 @@ +**Clean Windows installation acceptance — Original Leap Motion Controller** + +This check requires a separate Windows x64 PC with no previous Leap/Ultraleap +installation or driver. A fresh venv on the development PC does not provide that +evidence. Hosted CI verifies clean software installation and simulated behavior, +but has no USB controller attached. + +Use a supported 64-bit CPython 3.10–3.15 installation. Create a fresh venv outside +the source checkout, then install by package name from PyPI. Record the actual +package version before testing. + +```powershell +python -m venv leap-check +.\leap-check\Scripts\python.exe -m pip install source-robotics-leap-motion +.\leap-check\Scripts\source-leap.exe --version +.\leap-check\Scripts\source-leap.exe teleop --demo --headless --auto-arm --frames 30 +.\leap-check\Scripts\source-leap.exe doctor --json +``` + +The demo command requires b3 or later. If it is not yet published, use the prepared +release-candidate wheel explicitly and record that fact; a local-wheel test is not +the same as verifying the public package-name installation. + +Follow these steps with the physical controller: + +1. Start with the camera unplugged. Run `source-leap setup --json`. It should report + the absent camera distinctly from a missing driver; it must not install a driver + just because no frames arrived. +2. Plug in the Original controller. Inspect setup's device status. If Windows reports + a missing driver, run `source-leap setup --install-driver` and approve Windows' + administrator prompt. Record the driver publisher/version and every manual step. +3. Run `source-leap doctor --probe --json`, then `source-leap --latest --health --duration 10`. + Confirm live frames with no hands, then full hand data when a hand is visible. +4. Run `source-leap teleop`. Arm, clutch, resume, remove the hand, and return it. + Confirm a returning hand stays held until explicit rearming. Output goes to the + simulated Cartesian arm. +5. Close all package clients. Confirm the private engine shuts down after its final + client. Reopen tracking and repeat the frame probe. Check repeated startups and + one abrupt client exit. +6. Reuse the populated cache with network access unavailable. Tracking should work + without another download. Reboot and repeat the probe to verify driver persistence. + +Keep the console reports locally. Capture the package/interpreter versions, whether a vendor +service was present, runtime download/cache behavior, device/driver state, live frame +and skeleton counts, shutdown/restart outcome, and any unexpected errors. + +Passing means the package installs without a compiler or Python 3.8, first use obtains +the pinned runtime, setup diagnoses the device correctly, the minimum driver path +works, and tracking/restart/cache reuse succeed. A GUI vendor installation used to +make the test work must be recorded as an additional prerequisite rather than called +driver-only success. + +**Current evidence.** Package-name installation and live frames were verified in fresh +project-local venvs on an already working Windows PC. Private-engine startup/sharing/ +crash cleanup was tested separately with its existing driver. No separate clean +Windows USB installation has passed yet. This PC has no Windows Sandbox executable, +no installed WSL distribution, and no running Docker engine. Linux/macOS USB and actual +robot-controller checks also remain pending; their software CI results are separate. diff --git a/docs/TELEOPERATION.md b/docs/TELEOPERATION.md new file mode 100644 index 0000000..97092ac --- /dev/null +++ b/docs/TELEOPERATION.md @@ -0,0 +1,141 @@ +**Generic teleoperation — 0.1.0b3** + +The package now includes relative Cartesian control, a synchronous adapter contract, +and a browser lab with a simulated output. The lab is bundled with the Python package +and needs no web framework, numerical toolbox, robot SDK, or JavaScript build step. + +```powershell +.\.venv\Scripts\source-leap.exe teleop --demo +.\.venv\Scripts\source-leap.exe teleop +.\.venv\Scripts\source-leap.exe teleop --demo --config examples\teleoperation.toml +``` + +The first command uses a synthetic hand. The second uses the Leap controller. Both +send targets exclusively to `SimulatedArm`, which supplies immediate Cartesian +feedback. The viewer shows a workspace, tool position/orientation, recent path, +normalized gripper openness, frame rate, and local tracking age. It is a target +visualizer, with no robot joint geometry, inverse kinematics, collisions, or dynamics. + +Click **Arm & calibrate** with the selected hand visible. Move the hand to move the +tool target. Click **Clutch** to hold output while repositioning, then **Resume** to +capture the new hand reference. Holding Space also clutches; Escape disarms. Losing +focus while holding Space disarms. Hand loss requires explicit rearming. Reconnect +tracking explicitly after a device/service disconnect, then arm again. + +`--no-browser` prints the loopback URL without opening it. `--duration` and `--frames` +bound a run. `--headless --json` prints lab telemetry; `--auto-arm` explicitly requests +one initial simulated-arm calibration. It never rearms after loss. `--replay PATH` +uses recorded timing and does not need the controller. Default CLI bounds and speeds +belong to the simulation; a custom integration must supply its own configuration. + +**API and ownership.** The application owns a latest-mode `Tracker`; `Teleoperator` +owns the adapter inside its context. Controller and adapter methods run on the calling +application thread. Tracking acquisition keeps its separate owning worker. No robot +connection or additional thread is created by import or configuration construction. + +```python +from source_robotics_leap_motion import Pose, TeleoperationConfig, Teleoperator, Tracker, Vector3 +from source_robotics_leap_motion.adapters import SimulatedArm + +config = TeleoperationConfig.from_toml("examples/teleoperation.toml") +robot = SimulatedArm(Pose(Vector3(0.3, 0.0, 0.3))) + +with Tracker.demo(mode="latest", max_frame_age=config.max_frame_age) as tracker: + with Teleoperator(tracker, robot=robot, config=config) as teleop: + teleop.arm() + for _ in range(120): + teleop.step() +``` + +Use `CallbackAdapter(send=..., hold=..., feedback=..., open=..., close=...)` to wrap +an existing SDK. `send` receives an immutable `PoseTarget`. `hold(reason)` is mandatory +and must cancel or hold motion through that controller's interface. `feedback` returns +`RobotFeedback(Pose(...), gripper=...)`. Without feedback, pass an explicit known +reference to `arm(reference)` and `release_clutch(reference)`. Orientation/gripper +capabilities default to false and must be declared when implemented. Unsupported +configured commands are rejected before opening the adapter. + +The runnable [callback example](../examples/custom_robot.py) wraps the simulated +controller through these exact callbacks. Replacing that boundary is the integration +work for an arm. Cartesian controllers can accept mapped poses directly after unit +conversion. Joint-only controllers need kinematics and joint limits in their adapter +or robotics stack. The core implements Cartesian pose output; velocity/joint output +modes, SDK-specific adapters, and a network receiver are future additions. + +**Commands and conventions.** Positions and bounds use metres, linear speed uses +metres/second, and angular speed uses radians/second. `orientation=None` leaves +orientation to the controller; `gripper=None` leaves the gripper to the controller. +Enabled gripper commands and feedback are normalized to [0, 1]; the adapter converts +them to its own actuator units. Initial gripper feedback/reference is required when +gripper control is enabled. + +Commands include a random calibration session ID, increasing sequence, source frame +ID/timestamp/clock domain, and `valid_for_s`. A receiver uses its own monotonic receipt +clock to expire commands. Source timestamps do not synchronize machines. Adapters +must bound blocking SDK operations; application polling cannot stop a robot after +the application or network disappears. Implement expiry/hold in the receiving +controller and retain its own motion/limit enforcement. + +**Control behavior.** + +| State | Output and transition | +| --- | --- | +| `disarmed` | No target commands. Explicit arm calibrates one selected hand against supplied robot feedback/reference. | +| `active` | Distinct fresh hand frames can produce bounded targets. Arm itself emits no target. | +| `clutched` | Hold invoked; no target commands. Resume waits for a fresh frame, re-anchors to the supplied held robot reference, and emits nothing on that re-anchor frame. | +| `tracking_lost` | Hold invoked once on the loss transition. Returning frames do not rearm. | +| `fault` | Adapter/control error latched. Close and reopen before rearming. | +| `closed` | Hold and adapter close attempted; tracker ownership stays with the application. | + +Loss triggers include missing/ambiguous selected hands, hand identity changes, stale +local age, local update gaps, non-advancing/regressing source timestamps, source time +gaps, device changes, and clock-domain changes. Duplicates produce no output and do +not extend the tracking deadline. Empty-hand frames still mean the camera streams; +they also cause hand-based control to hold. Adapter exceptions preserve their original +cause; a failed send attempts hold before surfacing `AdapterError`. + +Position mapping is `robot_reference + gain * R * (hand - hand_reference)`, using +the configured proper axis rotation `R`. Each update clamps the desired position to +the workspace, smooths toward it, then bounds the norm of the target increment by +`max_linear_speed * elapsed`. Deadband uses the unsmoothed position error. This +limits commanded Cartesian targets; it does not model the robot's physical motion. + +`mapping.rotation` is an optional normalized x/y/z/w quaternion for arbitrary frame +alignment. It applies after signed axis assignment; gains apply in robot axes after +that rotation. The total `R` is used for position and orientation mapping together. +Its default is identity, preserving existing axis/gain-only configurations. + +Optional rotation mapping uses world-frame relative rotation: +`delta = q_hand * inverse(q_hand_reference)` and +`q_target = R * delta**rotation_gain * inverse(R) * q_robot_reference`. +The multiplication convention applies the rightmost rotation first, as documented +for [rotation composition](https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Rotation.__mul__.html). +Quaternion smoothing follows the shortest rotation path using +[spherical interpolation](https://docs.scipy.org/doc/scipy/reference/generated/scipy.spatial.transform.Slerp.html), +then caps the commanded angular increment. The implementation uses standard-library +math and has no SciPy dependency. Exponential smoothing uses elapsed local time and +`alpha = 1 - exp(-elapsed / time_constant)`. Zero time constant disables it. + +The output frequency is a maximum, not a periodic-delivery guarantee. Filtering and +speed history reset on calibration and clutch re-anchoring. Configured hand/clock +changes and long gaps require a new calibration. The control engine retains one +accepted frame, one hand anchor, and one command. Latest acquisition has one pending +frame; the lab retains the latest observed frame and a 240-point browser path. + +**Validation.** Tests cover calibration, configured axes/gains, no-output states, +tracking and identity loss, duplicate deadlines, clutch continuity, workspace/speed +bounds over a 500-frame trajectory, quaternion composition, angular limits, gripper +mapping, smoothing, command rate caps, adapter capabilities/failures/cleanup, strict +configuration, CLI telemetry, and packaged local HTTP assets/actions. Actual robot +motion and clean-PC driver installation remain separate hardware validation work. + +On Windows, the installed b3 wheel passed **180 tests on each of Python 3.10.11, +3.11.9, 3.12.10, 3.13.17, 3.14.10, and 3.15.2**. Each environment received 30 live +sequential frames and 30 latest-mode frames. The previously recorded 1,047-frame +session, containing 50 complete left-hand samples, generated 18 simulated targets +through a clutch/resume cycle. All targets stayed within the configured workspace +and speed bounds, and hand loss produced no further commands. Wheel metadata, +dependency checks, CLI examples, and the bundled viewer assets were also checked. +Chrome UI checks exercised calibration, target movement, clutch hold, and resume. +Raw local evidence is in `.tools/compatibility-results-0.1.0b3.json` and +`.tools/control-recording-b3.json`. No physical robot has been commanded. diff --git a/examples/custom_robot.py b/examples/custom_robot.py new file mode 100644 index 0000000..ddc6993 --- /dev/null +++ b/examples/custom_robot.py @@ -0,0 +1,48 @@ +"""Runnable callback integration using a simulated controller. + +Replace the adapter callbacks with your controller's SDK calls. Convert SI units +at that boundary, implement a real hold/cancel operation, and enforce command expiry +in the robot controller. The built-in example never connects to a physical robot. +""" + +import argparse +import time +from pathlib import Path + +from source_robotics_leap_motion import Pose, TeleoperationConfig, Teleoperator, Tracker +from source_robotics_leap_motion.adapters import CallbackAdapter, SimulatedArm + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--demo", action="store_true", help="synthetic hand instead of camera") + parser.add_argument("--duration", type=float, default=10) + args = parser.parse_args() + config = TeleoperationConfig.from_toml(Path(__file__).with_name("teleoperation.toml")) + center = (config.workspace.minimum + config.workspace.maximum) * 0.5 + controller = SimulatedArm(Pose(center)) + robot = CallbackAdapter( + send=controller.send, + hold=controller.hold, + feedback=controller.read_feedback, + open=controller.open, + close=controller.close, + orientation=True, + gripper=True, + ) + factory = Tracker.demo if args.demo else Tracker + with factory(mode="latest", max_frame_age=config.max_frame_age) as tracker: + with Teleoperator(tracker, robot=robot, config=config) as teleop: + teleop.arm() # Explicit calibration using fresh hand data and robot feedback. + started = time.monotonic() + while time.monotonic() - started < args.duration: + target = teleop.step() + if target is not None: + print(target.position) + if teleop.status.state == "tracking_lost": + print("Output held:", teleop.status.reason) + break # Deliberate rearming is required after loss. + + +if __name__ == "__main__": + main() diff --git a/examples/teleoperation.toml b/examples/teleoperation.toml new file mode 100644 index 0000000..d612824 --- /dev/null +++ b/examples/teleoperation.toml @@ -0,0 +1,30 @@ +# Example bounds for the simulated arm. Supply your own bounds for a real robot. +[workspace] +minimum = [0.0, -0.3, 0.0] +maximum = [0.6, 0.3, 0.6] + +[mapping] +axes = ["-z", "-x", "y"] +# Optional arbitrary frame rotation after axis assignment; normalized x/y/z/w. +rotation = [0.0, 0.0, 0.0, 1.0] +gain = [0.75, 0.75, 0.75] + +[teleoperation] +handedness = "right" +max_linear_speed = 0.2 # metres/second, norm of Cartesian target movement +control_orientation = true +rotation_gain = 1.0 +max_angular_speed = 1.0 # radians/second +smoothing_time_constant = 0.05 # seconds; zero disables smoothing +position_deadband = 0.001 # metres +max_frame_age = 0.1 # seconds since local receipt +tracking_timeout = 0.15 # seconds between valid updates +max_output_hz = 60.0 # maximum command frequency, not a fixed-rate guarantee + +# Omit this table to disable gripper control. +[gripper] +source = "grab" # "grab" or "pinch" +open_value = 1.0 +closed_value = 0.0 +max_speed = 1.0 # normalized units/second +deadband = 0.01 diff --git a/pyproject.toml b/pyproject.toml index 78d8793..050e0dd 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "hatchling.build" [project] name = "source-robotics-leap-motion" -version = "0.1.0b2" +version = "0.1.0b3" description = "Modern Leap Motion tracking and configurable robot-arm teleoperation primitives" readme = "README.md" requires-python = ">=3.10" diff --git a/src/source_robotics_leap_motion/__init__.py b/src/source_robotics_leap_motion/__init__.py index 3895799..5c8e5e3 100644 --- a/src/source_robotics_leap_motion/__init__.py +++ b/src/source_robotics_leap_motion/__init__.py @@ -1,38 +1,56 @@ """Leap Motion tracking for modern Python. Importing never opens hardware.""" +from .control import TeleoperationController, TeleoperationStatus +from .control_config import GripperMapping, TeleoperationConfig, Workspace +from .control_models import Pose, PoseTarget, RobotCapabilities, RobotFeedback from .errors import ( + AdapterError, ConnectionError, LeapError, RecordingError, RuntimeNotFoundError, StaleFrameError, + TeleoperationError, TrackingTimeoutError, ) from .health import TrackingHealth from .mapping import Mapping from .models import Bone, Finger, Frame, Hand, Palm, Quaternion, Vector3 from .recording import Recorder, read_recording +from .teleoperation import Teleoperator from .tracker import Tracker -__version__ = "0.1.0b2" +__version__ = "0.1.0b3" __all__ = [ + "AdapterError", "Bone", "ConnectionError", "Finger", "Frame", "Hand", + "GripperMapping", "LeapError", "Mapping", "Palm", + "Pose", + "PoseTarget", "Quaternion", "Recorder", "RecordingError", "RuntimeNotFoundError", + "RobotCapabilities", + "RobotFeedback", "StaleFrameError", "Tracker", "TrackingHealth", "TrackingTimeoutError", + "TeleoperationConfig", + "TeleoperationController", + "TeleoperationError", + "TeleoperationStatus", + "Teleoperator", "Vector3", + "Workspace", "read_recording", ] diff --git a/src/source_robotics_leap_motion/_geometry.py b/src/source_robotics_leap_motion/_geometry.py new file mode 100644 index 0000000..f8fc48c --- /dev/null +++ b/src/source_robotics_leap_motion/_geometry.py @@ -0,0 +1,90 @@ +"""Small quaternion operations, independent of numerical or robot SDKs.""" + +import math + +from .models import Quaternion, Vector3 + + +def rotate(q: Quaternion, v: Vector3) -> Vector3: + q = unit(q.x, q.y, q.z, q.w) + t = Vector3(q.y * v.z - q.z * v.y, q.z * v.x - q.x * v.z, q.x * v.y - q.y * v.x) * 2 + return ( + v + t * q.w + Vector3(q.y * t.z - q.z * t.y, q.z * t.x - q.x * t.z, q.x * t.y - q.y * t.x) + ) + + +def unit(x: float, y: float, z: float, w: float) -> Quaternion: + length = math.sqrt(x * x + y * y + z * z + w * w) + return Quaternion(x / length, y / length, z / length, w / length) + + +def inverse(q: Quaternion) -> Quaternion: + return unit(-q.x, -q.y, -q.z, q.w) + + +def multiply(a: Quaternion, b: Quaternion) -> Quaternion: + return unit( + a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, + a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x, + a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w, + a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z, + ) + + +def angle(a: Quaternion, b: Quaternion) -> float: + a = unit(a.x, a.y, a.z, a.w) + b = unit(b.x, b.y, b.z, b.w) + dot = abs(a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w) + return 2 * math.acos(min(1.0, dot)) + + +def slerp(a: Quaternion, b: Quaternion, fraction: float) -> Quaternion: + a = unit(a.x, a.y, a.z, a.w) + b = unit(b.x, b.y, b.z, b.w) + av = (a.x, a.y, a.z, a.w) + bv = (b.x, b.y, b.z, b.w) + dot = sum(x * y for x, y in zip(av, bv, strict=True)) + if dot < 0: + bv = tuple(-value for value in bv) + dot = -dot + if dot > 0.9995: + return unit(*(x + fraction * (y - x) for x, y in zip(av, bv, strict=True))) + theta = math.acos(min(1.0, dot)) + left = math.sin((1 - fraction) * theta) / math.sin(theta) + right = math.sin(fraction * theta) / math.sin(theta) + return unit(*(left * x + right * y for x, y in zip(av, bv, strict=True))) + + +def power(q: Quaternion, gain: float) -> Quaternion: + q = unit(q.x, q.y, q.z, q.w) + sign = 1 if q.w >= 0 else -1 + sine = math.sqrt(q.x * q.x + q.y * q.y + q.z * q.z) + if sine < 1e-12: + return Quaternion() + half_angle = math.atan2(sine, abs(q.w)) * gain + scale = sign * math.sin(half_angle) / sine + return unit(q.x * scale, q.y * scale, q.z * scale, math.cos(half_angle)) + + +def axis_rotation(axes: tuple[str, str, str]) -> Quaternion: + matrix = [[0.0] * 3 for _ in range(3)] + for row, axis in enumerate(axes): + matrix[row]["xyz".index(axis.lstrip("-"))] = -1 if axis.startswith("-") else 1 + trace = sum(matrix[i][i] for i in range(3)) + if trace > 0: + scale = math.sqrt(trace + 1) * 2 + return unit( + (matrix[2][1] - matrix[1][2]) / scale, + (matrix[0][2] - matrix[2][0]) / scale, + (matrix[1][0] - matrix[0][1]) / scale, + scale / 4, + ) + i = max(range(3), key=lambda j: matrix[j][j]) + j, k = (i + 1) % 3, (i + 2) % 3 + scale = math.sqrt(1 + matrix[i][i] - matrix[j][j] - matrix[k][k]) * 2 + values = [0.0] * 4 + values[i] = scale / 4 + values[j] = (matrix[j][i] + matrix[i][j]) / scale + values[k] = (matrix[k][i] + matrix[i][k]) / scale + values[3] = (matrix[k][j] - matrix[j][k]) / scale + return unit(*values) diff --git a/src/source_robotics_leap_motion/adapters/__init__.py b/src/source_robotics_leap_motion/adapters/__init__.py new file mode 100644 index 0000000..27cf984 --- /dev/null +++ b/src/source_robotics_leap_motion/adapters/__init__.py @@ -0,0 +1,7 @@ +"""Robot integration contracts; imports never connect to a controller.""" + +from .base import RobotAdapter +from .callback import CallbackAdapter +from .simulated import SimulatedArm + +__all__ = ["CallbackAdapter", "RobotAdapter", "SimulatedArm"] diff --git a/src/source_robotics_leap_motion/adapters/base.py b/src/source_robotics_leap_motion/adapters/base.py new file mode 100644 index 0000000..bfadf86 --- /dev/null +++ b/src/source_robotics_leap_motion/adapters/base.py @@ -0,0 +1,15 @@ +"""Cartesian adapter contract. Joint-only controllers supply their own kinematics.""" + +from typing import Protocol + +from ..control_models import PoseTarget, RobotCapabilities, RobotFeedback + + +class RobotAdapter(Protocol): + capabilities: RobotCapabilities + + def open(self) -> None: ... + def read_feedback(self) -> RobotFeedback | None: ... + def send(self, target: PoseTarget) -> None: ... + def hold(self, reason: str) -> None: ... + def close(self) -> None: ... diff --git a/src/source_robotics_leap_motion/adapters/callback.py b/src/source_robotics_leap_motion/adapters/callback.py new file mode 100644 index 0000000..4a329f1 --- /dev/null +++ b/src/source_robotics_leap_motion/adapters/callback.py @@ -0,0 +1,60 @@ +"""Integrate an existing controller through explicit target and hold callbacks.""" + +from collections.abc import Callable + +from ..control_models import PoseTarget, RobotCapabilities, RobotFeedback +from ..errors import AdapterError + + +class CallbackAdapter: + def __init__( + self, + send: Callable[[PoseTarget], None], + *, + hold: Callable[[str], None], + feedback: Callable[[], RobotFeedback] | None = None, + open: Callable[[], None] | None = None, + close: Callable[[], None] | None = None, + orientation: bool = False, + gripper: bool = False, + ) -> None: + self.capabilities = RobotCapabilities( + orientation=orientation, gripper=gripper, feedback=feedback is not None + ) + self._send, self._hold, self._feedback = send, hold, feedback + self._open, self._close = open, close + self._opened = False + + def open(self) -> None: + if self._opened: + raise AdapterError("Callback adapter is already open") + # Mark the attempt so close can release a partially opened controller. + self._opened = True + if self._open is not None: + self._open() + + def read_feedback(self) -> RobotFeedback | None: + self._require_open() + return None if self._feedback is None else self._feedback() + + def send(self, target: PoseTarget) -> None: + self._require_open() + if target.orientation is not None and not self.capabilities.orientation: + raise AdapterError("Adapter does not support orientation commands") + if target.gripper is not None and not self.capabilities.gripper: + raise AdapterError("Adapter does not support gripper commands") + self._send(target) + + def hold(self, reason: str) -> None: + self._require_open() + self._hold(reason) + + def close(self) -> None: + if self._opened: + self._opened = False + if self._close is not None: + self._close() + + def _require_open(self) -> None: + if not self._opened: + raise AdapterError("Open the callback adapter first") diff --git a/src/source_robotics_leap_motion/adapters/simulated.py b/src/source_robotics_leap_motion/adapters/simulated.py new file mode 100644 index 0000000..f6fc711 --- /dev/null +++ b/src/source_robotics_leap_motion/adapters/simulated.py @@ -0,0 +1,45 @@ +"""Immediate Cartesian feedback for demos; no joint geometry or physical dynamics.""" + +from ..control_models import Pose, PoseTarget, RobotCapabilities, RobotFeedback +from ..errors import AdapterError + + +class SimulatedArm: + capabilities = RobotCapabilities(orientation=True, gripper=True, feedback=True) + + def __init__(self, initial_pose: Pose, *, gripper: float = 1.0) -> None: + self._feedback = RobotFeedback(initial_pose, gripper) + self.last_target: PoseTarget | None = None + self.targets_sent = 0 + self.hold_reason = "not_open" + self._opened = False + + def open(self) -> None: + if self._opened: + raise AdapterError("Simulated arm is already open") + self._opened = True + + def read_feedback(self) -> RobotFeedback: + self._require_open() + return self._feedback + + def send(self, target: PoseTarget) -> None: + self._require_open() + self._feedback = RobotFeedback( + Pose(target.position, target.orientation or self._feedback.pose.orientation), + self._feedback.gripper if target.gripper is None else target.gripper, + ) + self.last_target = target + self.targets_sent += 1 + self.hold_reason = "" + + def hold(self, reason: str) -> None: + self._require_open() + self.hold_reason = reason + + def close(self) -> None: + self._opened = False + + def _require_open(self) -> None: + if not self._opened: + raise AdapterError("Open the simulated arm first") diff --git a/src/source_robotics_leap_motion/cli.py b/src/source_robotics_leap_motion/cli.py index f6c0f76..5f87dd8 100644 --- a/src/source_robotics_leap_motion/cli.py +++ b/src/source_robotics_leap_motion/cli.py @@ -106,6 +106,26 @@ def _parser() -> argparse.ArgumentParser: replay.add_argument("--json", action="store_true") replay.add_argument("--health", action="store_true") replay.add_argument("--timeout", type=_positive_float, default=5) + lab = commands.add_parser("teleop", help="inspect hand control with a simulated Cartesian arm") + lab.add_argument("--demo", action="store_true", help="use a synthetic hand") + lab.add_argument("--replay", help="use a recorded hand session at recorded speed") + lab.add_argument("--config", help="teleoperation TOML with workspace and speed limits") + lab.add_argument("--headless", action="store_true", help="run without the browser viewer") + lab.add_argument( + "--auto-arm", action="store_true", help="automatically calibrate the simulated arm once" + ) + lab.add_argument("--json", action="store_true", help="headless lab telemetry as JSON Lines") + lab.add_argument( + "--no-browser", action="store_true", help="print the viewer URL without opening it" + ) + lab.add_argument( + "--port", type=int, default=0, help="loopback viewer port; zero selects a free port" + ) + lab.add_argument("--duration", type=_positive_float) + lab.add_argument("--frames", type=_positive_int) + runtime = lab.add_mutually_exclusive_group() + runtime.add_argument("--library") + runtime.add_argument("--runtime-dir") runtime = commands.add_parser("runtime", help="prepare or inspect a local runtime prototype") actions = runtime.add_subparsers(dest="runtime_action", required=True) prepare = actions.add_parser( @@ -360,6 +380,7 @@ def main(argv: list[str] | None = None) -> int: "record", "replay", "runtime", + "teleop", "--version", "--help", "-h", @@ -375,6 +396,10 @@ def main(argv: list[str] | None = None) -> int: return _doctor(args) if args.command == "runtime": return _runtime(args) + if args.command == "teleop": + from .lab import run_lab + + return run_lab(args) return _run(args) except KeyboardInterrupt: print("Stopped.", file=sys.stderr) diff --git a/src/source_robotics_leap_motion/control.py b/src/source_robotics_leap_motion/control.py new file mode 100644 index 0000000..717fd29 --- /dev/null +++ b/src/source_robotics_leap_motion/control.py @@ -0,0 +1,280 @@ +"""Deterministic relative Cartesian control; this module performs no robot I/O.""" + +from __future__ import annotations + +import math +import time +import uuid +from dataclasses import dataclass +from typing import Literal + +from ._geometry import angle, axis_rotation, inverse, multiply, power, slerp +from .control_config import TeleoperationConfig +from .control_models import Pose, PoseTarget, RobotFeedback +from .errors import TeleoperationError +from .models import Frame, Hand + +ControlState = Literal["disarmed", "active", "clutched", "tracking_lost", "fault", "closed"] + + +@dataclass(frozen=True, slots=True) +class TeleoperationStatus: + state: ControlState + reason: str + session_id: str | None + sequence: int + hand_id: int | None + last_target: PoseTarget | None + workspace_limited: bool = False + speed_limited: bool = False + + +class TeleoperationController: + """Call arm explicitly, then update with fresh frames using one application thread. + + The caller supplies local receipt age. The managed Teleoperator integrates this + with Tracker's latest mode. No recovery path automatically rearms the controller. + Local `now` values must come from one monotonic clock; source timestamps are only + compared with other timestamps in that same source clock domain. + """ + + def __init__(self, config: TeleoperationConfig) -> None: + self.config = config + self._state: ControlState = "disarmed" + self._reason = "not_armed" + self._session: str | None = None + self._sequence = 0 + self._hand: Hand | None = None + self._reference: RobotFeedback | None = None + self._pose: Pose | None = None + self._gripper: float | None = None + self._last_target: PoseTarget | None = None + self._last_frame: Frame | None = None + self._last_valid_at: float | None = None + self._last_output_at: float | None = None + self._last_now: float | None = None + self._resume: RobotFeedback | None = None + self._workspace_limited = self._speed_limited = False + self._rotation = multiply(config.mapping.rotation, axis_rotation(config.mapping.axes)) + + @property + def status(self) -> TeleoperationStatus: + return TeleoperationStatus( + self._state, + self._reason, + self._session, + self._sequence, + None if self._hand is None else self._hand.id, + self._last_target, + self._workspace_limited, + self._speed_limited, + ) + + def _now(self, now: float | None) -> float: + value = time.monotonic() if now is None else now + if not math.isfinite(value) or value < 0: + self.fault("invalid_control_time") + raise ValueError("Control time must be finite and non-negative") + if self._last_now is not None and value < self._last_now: + self.fault("local_clock_regressed") + raise TeleoperationError("Control time moved backwards") + self._last_now = value + return value + + def _select(self, frame: Frame) -> Hand | None: + hands = [hand for hand in frame.hands if hand.handedness == self.config.handedness] + return hands[0] if len(hands) == 1 else None + + def _check_reference(self, reference: RobotFeedback) -> None: + if not isinstance(reference, RobotFeedback): + raise TeleoperationError("Supply robot feedback or an explicit RobotFeedback reference") + if not self.config.workspace.contains(reference.pose.position): + raise TeleoperationError("Robot reference pose is outside the configured workspace") + if self.config.gripper is not None and reference.gripper is None: + raise TeleoperationError("Gripper control requires an explicit gripper reference") + + def _anchor(self, hand: Hand, reference: RobotFeedback, now: float) -> None: + self._hand = hand + self._reference = reference + self._pose = reference.pose + self._gripper = reference.gripper + self._last_output_at = now + self._workspace_limited = self._speed_limited = False + + def arm( + self, + frame: Frame, + reference: RobotFeedback, + *, + now: float | None = None, + frame_age: float = 0.0, + ) -> None: + if self._state not in ("disarmed", "tracking_lost"): + raise TeleoperationError("Disarm before calibrating and arming a new session") + now = self._now(now) + self._check_reference(reference) + if not math.isfinite(frame_age) or not 0 <= frame_age <= self.config.max_frame_age: + raise TeleoperationError("Calibration requires a fresh frame") + hand = self._select(frame) + if hand is None: + raise TeleoperationError( + f"Calibration requires exactly one {self.config.handedness} hand" + ) + self._anchor(hand, reference, now) + self._last_frame = frame + self._last_valid_at = now - frame_age + self._last_target = None + self._resume = None + self._sequence = 0 + self._session = uuid.uuid4().hex + self._state, self._reason = "active", "armed" + + def disarm(self, reason: str = "disarmed") -> None: + if self._state in ("fault", "closed"): + return + self._state, self._reason = "disarmed", reason + self._resume = None + + def tracking_lost(self, reason: str = "tracking_lost") -> None: + if self._state in ("active", "clutched"): + self._state, self._reason = "tracking_lost", reason + self._resume = None + + def fault(self, reason: str) -> None: + self._state, self._reason = "fault", reason + self._resume = None + + def close(self) -> None: + self._state, self._reason = "closed", "closed" + self._resume = None + + def clutch(self) -> None: + if self._state != "active": + raise TeleoperationError("Clutch requires an active session") + self._state, self._reason = "clutched", "clutch_engaged" + + def release_clutch(self, reference: RobotFeedback) -> None: + if self._state != "clutched": + raise TeleoperationError("Clutch release requires a clutched session") + self._check_reference(reference) + self._resume = reference + self._reason = "waiting_for_reanchor" + + def update( + self, + frame: Frame | None, + *, + now: float | None = None, + frame_age: float = 0.0, + ) -> PoseTarget | None: + now = self._now(now) + if self._state not in ("active", "clutched"): + return None + if ( + self._last_valid_at is not None + and now - self._last_valid_at > self.config.tracking_timeout + ): + self.tracking_lost("tracking_gap") + return None + if not math.isfinite(frame_age) or frame_age < 0: + self.fault("invalid_frame_age") + raise ValueError("Frame age must be finite and non-negative") + if frame is None: + return None + if frame_age > self.config.max_frame_age: + self.tracking_lost("stale_frame") + return None + previous = self._last_frame + if previous is not None: + if (frame.clock_domain, frame.device_id) != (previous.clock_domain, previous.device_id): + self.tracking_lost("source_changed") + return None + if (frame.id, frame.timestamp_us) == (previous.id, previous.timestamp_us): + return None + delta = (frame.timestamp_us - previous.timestamp_us) / 1_000_000 + if delta <= 0 or delta > self.config.tracking_timeout: + self.tracking_lost("source_clock_reset" if delta <= 0 else "source_gap") + return None + hand = self._select(frame) + if hand is None: + self.tracking_lost("hand_missing") + return None + if self._hand is None or hand.id != self._hand.id: + self.tracking_lost("hand_changed") + return None + self._last_frame = frame + self._last_valid_at = now - frame_age + if self._state == "clutched": + if self._resume is not None: + self._anchor(hand, self._resume, now) + self._resume = None + self._state, self._reason = "active", "clutch_reanchored" + return None + if now - self._last_output_at + 1e-12 < 1 / self.config.max_output_hz: + return None + try: + return self._target(frame, hand, now) + except Exception: + self.fault("invalid_target") + raise + + def _target(self, frame: Frame, hand: Hand, now: float) -> PoseTarget: + config = self.config + dt = now - self._last_output_at + alpha = ( + 1.0 + if config.smoothing_time_constant == 0 + else -math.expm1(-dt / config.smoothing_time_constant) + ) + desired = config.mapping.map_position( + hand.palm.position, + hand_origin=self._hand.palm.position, + robot_origin=self._reference.pose.position, + ) + bounded = config.workspace.clamp(desired) + self._workspace_limited = desired != bounded + delta = (bounded - self._pose.position) * alpha + if (bounded - self._pose.position).norm <= config.position_deadband: + delta = delta * 0 + limit = config.max_linear_speed * dt + self._speed_limited = delta.norm > limit + if delta.norm > limit: + delta = delta * (limit / delta.norm) + position = config.workspace.clamp(self._pose.position + delta) + orientation = self._pose.orientation + if config.control_orientation: + camera_delta = multiply(hand.palm.orientation, inverse(self._hand.palm.orientation)) + robot_delta = multiply(multiply(self._rotation, camera_delta), inverse(self._rotation)) + desired_orientation = multiply( + power(robot_delta, config.rotation_gain), self._reference.pose.orientation + ) + smoothed = slerp(orientation, desired_orientation, alpha) + distance = angle(orientation, smoothed) + fraction = min(1.0, config.max_angular_speed * dt / distance) if distance > 0 else 1.0 + self._speed_limited |= fraction < 1 + orientation = slerp(orientation, smoothed, fraction) + if config.gripper is not None: + strength = ( + hand.grab_strength if config.gripper.source == "grab" else hand.pinch_strength + ) + goal = config.gripper.map(strength) + delta_grip = (goal - self._gripper) * alpha + if abs(goal - self._gripper) <= config.gripper.deadband: + delta_grip = 0.0 + step = config.gripper.max_speed * dt + self._gripper += max(-step, min(step, delta_grip)) + self._pose = Pose(position, orientation) + self._sequence += 1 + self._last_output_at = now + self._last_target = PoseTarget( + position, + orientation if config.control_orientation else None, + self._gripper if config.gripper is not None else None, + self._session, + self._sequence, + frame.id, + frame.timestamp_us, + frame.clock_domain, + min(config.max_frame_age, config.tracking_timeout), + ) + return self._last_target diff --git a/src/source_robotics_leap_motion/control_config.py b/src/source_robotics_leap_motion/control_config.py new file mode 100644 index 0000000..005748b --- /dev/null +++ b/src/source_robotics_leap_motion/control_config.py @@ -0,0 +1,150 @@ +"""Strict, dependency-free teleoperation settings in SI units.""" + +from __future__ import annotations + +import math +from dataclasses import dataclass +from pathlib import Path +from typing import Literal + +from ._compat import tomllib +from .mapping import Mapping, _parse_rotation +from .models import Vector3 + + +def _number(value: float, name: str, *, zero: bool = False) -> None: + if isinstance(value, bool) or not isinstance(value, (int, float)) or not math.isfinite(value): + raise ValueError(f"{name} must be a finite number") + if value < 0 or (value == 0 and not zero): + raise ValueError(f"{name} must be {'non-negative' if zero else 'positive'}") + + +@dataclass(frozen=True, slots=True) +class Workspace: + minimum: Vector3 + maximum: Vector3 + + def __post_init__(self) -> None: + if any(a > b for a, b in zip(self.minimum, self.maximum, strict=True)): + raise ValueError("Workspace minimum must not exceed maximum") + + def clamp(self, position: Vector3) -> Vector3: + return Vector3( + *( + max(lo, min(hi, x)) + for x, lo, hi in zip(position, self.minimum, self.maximum, strict=True) + ) + ) + + def contains(self, position: Vector3) -> bool: + return self.clamp(position) == position + + +@dataclass(frozen=True, slots=True) +class GripperMapping: + source: Literal["grab", "pinch"] = "grab" + open_value: float = 1.0 + closed_value: float = 0.0 + max_speed: float = 1.0 + deadband: float = 0.01 + + def __post_init__(self) -> None: + if self.source not in ("grab", "pinch"): + raise ValueError("Gripper source must be 'grab' or 'pinch'") + for name in ("open_value", "closed_value", "deadband"): + value = getattr(self, name) + _number(value, name, zero=True) + if value > 1: + raise ValueError(f"{name} must be in [0, 1]") + _number(self.max_speed, "Gripper max_speed") + + def map(self, strength: float) -> float: + return self.open_value + (self.closed_value - self.open_value) * strength + + +@dataclass(frozen=True, slots=True) +class TeleoperationConfig: + workspace: Workspace + max_linear_speed: float + mapping: Mapping = Mapping() + handedness: Literal["left", "right"] = "right" + smoothing_time_constant: float = 0.05 + position_deadband: float = 0.001 + control_orientation: bool = False + rotation_gain: float = 1.0 + max_angular_speed: float = 1.0 + gripper: GripperMapping | None = None + max_frame_age: float = 0.1 + tracking_timeout: float = 0.15 + max_output_hz: float = 60.0 + + def __post_init__(self) -> None: + if self.handedness not in ("left", "right"): + raise ValueError("Handedness must be 'left' or 'right'") + if not isinstance(self.control_orientation, bool): + raise ValueError("control_orientation must be true or false") + for name in ( + "max_linear_speed", + "max_angular_speed", + "max_frame_age", + "tracking_timeout", + "max_output_hz", + ): + _number(getattr(self, name), name) + for name in ("smoothing_time_constant", "position_deadband", "rotation_gain"): + _number(getattr(self, name), name, zero=True) + if self.max_output_hz > 1000: + raise ValueError("max_output_hz must not exceed 1000") + if self.max_frame_age > self.tracking_timeout: + raise ValueError("max_frame_age must not exceed tracking_timeout") + + @classmethod + def from_toml(cls, path: str | Path) -> TeleoperationConfig: + with Path(path).open("rb") as stream: + document = tomllib.load(stream) + tables = {"mapping", "workspace", "teleoperation", "gripper"} + if set(document) - tables: + raise ValueError("Unknown configuration tables: " + ", ".join(set(document) - tables)) + if not {"workspace", "teleoperation"} <= set(document): + raise ValueError("Configuration requires [workspace] and [teleoperation] tables") + for name, table in document.items(): + if not isinstance(table, dict): + raise ValueError(f"{name} must be a table") + workspace = document["workspace"] + if set(workspace) != {"minimum", "maximum"}: + raise ValueError("Workspace requires exactly minimum and maximum") + mapping = document.get("mapping", {}) + if set(mapping) - {"axes", "gain", "rotation"}: + raise ValueError("Unknown mapping settings") + axes = mapping.get("axes", ["x", "y", "z"]) + if not isinstance(axes, list) or any(not isinstance(axis, str) for axis in axes): + raise ValueError("Mapping axes must be an array of axis names") + rotation = _parse_rotation(mapping.get("rotation", [0, 0, 0, 1])) + values = dict(document["teleoperation"]) + if "max_linear_speed" not in values: + raise ValueError("teleoperation.max_linear_speed is required") + allowed = set(cls.__dataclass_fields__) - {"workspace", "mapping", "gripper"} + if set(values) - allowed: + raise ValueError("Unknown teleoperation settings: " + ", ".join(set(values) - allowed)) + gripper = document.get("gripper") + if gripper is not None and set(gripper) - set(GripperMapping.__dataclass_fields__): + raise ValueError("Unknown gripper settings") + return cls( + workspace=Workspace(_vector(workspace["minimum"]), _vector(workspace["maximum"])), + mapping=Mapping(tuple(axes), _vector(mapping.get("gain", [1, 1, 1])), rotation), + gripper=None if gripper is None else GripperMapping(**gripper), + **values, + ) + + +def _vector(values) -> Vector3: + if not isinstance(values, list) or len(values) != 3: + raise ValueError("Configuration vectors require three numbers") + for value in values: + if ( + isinstance(value, bool) + or not isinstance(value, (int, float)) + or not math.isfinite(value) + ): + raise ValueError("Configuration vectors require finite numbers") + return Vector3(*values) diff --git a/src/source_robotics_leap_motion/control_models.py b/src/source_robotics_leap_motion/control_models.py new file mode 100644 index 0000000..83ad609 --- /dev/null +++ b/src/source_robotics_leap_motion/control_models.py @@ -0,0 +1,55 @@ +"""Owned Cartesian commands and explicit robot integration capabilities.""" + +from dataclasses import dataclass + +from .models import Quaternion, Vector3 + + +@dataclass(frozen=True, slots=True) +class Pose: + position: Vector3 + orientation: Quaternion = Quaternion() + + def __post_init__(self) -> None: + if not isinstance(self.position, Vector3) or not isinstance(self.orientation, Quaternion): + raise ValueError("A Pose requires a Vector3 position and Quaternion orientation") + + +@dataclass(frozen=True, slots=True) +class RobotFeedback: + pose: Pose + gripper: float | None = None + + def __post_init__(self) -> None: + if not isinstance(self.pose, Pose): + raise ValueError("Robot feedback must contain a Pose") + if self.gripper is not None and not 0 <= self.gripper <= 1: + raise ValueError("Gripper feedback must be normalized to [0, 1]") + + +@dataclass(frozen=True, slots=True) +class PoseTarget: + """A Cartesian command; None leaves orientation/gripper under robot control. + + Positions are metres, orientation is x/y/z/w in the configured robot frame, + and gripper values are normalized. Receivers expire commands using their own + monotonic receipt time and ``valid_for_s``, never the source clock. + """ + + position: Vector3 + orientation: Quaternion | None + gripper: float | None + session_id: str + sequence: int + source_frame_id: int + source_timestamp_us: int + clock_domain: str + valid_for_s: float + + +@dataclass(frozen=True, slots=True) +class RobotCapabilities: + cartesian_position: bool = True + orientation: bool = False + gripper: bool = False + feedback: bool = False diff --git a/src/source_robotics_leap_motion/errors.py b/src/source_robotics_leap_motion/errors.py index dfbbbff..01d9378 100644 --- a/src/source_robotics_leap_motion/errors.py +++ b/src/source_robotics_leap_motion/errors.py @@ -23,3 +23,11 @@ class StaleFrameError(TrackingTimeoutError): class RecordingError(LeapError): """A recording is invalid, unsupported, or truncated.""" + + +class TeleoperationError(LeapError): + """Invalid control state, calibration, or robot integration.""" + + +class AdapterError(TeleoperationError): + """Robot adapter operation failed; inspect the chained original exception.""" diff --git a/src/source_robotics_leap_motion/lab.css b/src/source_robotics_leap_motion/lab.css new file mode 100644 index 0000000..cbb42cd --- /dev/null +++ b/src/source_robotics_leap_motion/lab.css @@ -0,0 +1,511 @@ +:root { + font-family: + Inter, + Segoe UI, + Arial, + sans-serif; + color: #172b36; + background: #eef2f4; + font-synthesis: none; + --green: #177d67; + --line: #dde5e9; +} +* { + box-sizing: border-box; +} +body { + margin: 0; +} +button { + font: inherit; + cursor: pointer; +} +header { + height: 80px; + background: #fff; + border-bottom: 1px solid var(--line); + display: flex; + align-items: center; + justify-content: space-between; + padding: 0 38px; +} +.brand { + font-weight: 800; + font-size: 15px; + letter-spacing: 2px; + display: flex; + align-items: center; + gap: 13px; +} +.mark { + width: 30px; + height: 30px; + background: var(--green); + border-radius: 9px; + color: white; + display: grid; + place-items: center; + font-size: 19px; +} +.tag { + font-size: 11px; + letter-spacing: 1.4px; + background: #eaf4f0; + border: 1px solid #cee6dd; + color: #176e57; + border-radius: 30px; + padding: 9px 13px; + font-weight: 700; +} +main { + max-width: 1490px; + padding: 35px 38px; + margin: auto; +} +.heading { + display: flex; + align-items: end; + justify-content: space-between; + margin-bottom: 26px; +} +.eyebrow { + font-size: 11px; + font-weight: 700; + letter-spacing: 2px; + color: #6a7c88; + margin-bottom: 10px; +} +h1 { + font-size: 33px; + font-weight: 650; + letter-spacing: -1px; + margin: 0; +} +h2 { + font-size: 15px; + margin: 0; +} +.subtitle { + color: #74838b; + font-size: 14px; + margin: 10px 0 0; +} +.connection { + font-size: 12px; + color: #6b7f87; +} +.dot { + display: inline-block; + background: #94a7ad; + width: 7px; + height: 7px; + border-radius: 50%; + margin-right: 7px; +} +.dot.on { + background: #1b9b77; + box-shadow: 0 0 0 4px #e4f4ec; +} +.layout { + display: grid; + grid-template-columns: 305px minmax(400px, 1fr); + gap: 23px; +} +.card { + background: white; + border: 1px solid var(--line); + border-radius: 16px; + box-shadow: 0 3px 12px #223a4a03; +} +.side { + display: grid; + gap: 17px; + align-content: start; +} +.section { + padding: 23px; +} +.section-label { + font-size: 10px; + letter-spacing: 1.7px; + font-weight: 750; + color: #7b8c95; + margin-bottom: 16px; +} +.source { + display: flex; + align-items: center; + gap: 12px; + margin-bottom: 20px; +} +.source-icon { + background: #eaf3f0; + border-radius: 11px; + width: 42px; + height: 42px; + display: grid; + place-items: center; + color: #177d67; + font-size: 22px; +} +.source-name { + font-size: 14px; + font-weight: 650; + text-transform: capitalize; +} +.source-note { + font-size: 12px; + color: #819099; + margin-top: 4px; +} +.stats { + display: grid; + grid-template-columns: 1fr 1fr; + gap: 16px; +} +.stats strong { + font-size: 24px; + font-weight: 600; + letter-spacing: -1px; +} +.stats span { + font-size: 11px; + color: #7c8d97; + display: block; + margin-top: 5px; +} +.unit { + font-size: 12px; + color: #8b99a0; + margin-left: 3px; +} +.state-row { + display: flex; + align-items: center; + justify-content: space-between; + gap: 8px; +} +.badge { + font-size: 10px; + letter-spacing: 1px; + border-radius: 6px; + padding: 7px 9px; + background: #edf1f4; + color: #667c87; + font-weight: 750; +} +.badge.active { + background: #dcf4e7; + color: #177651; +} +.badge.clutched { + background: #fff0d2; + color: #9d6c14; +} +.badge.tracking_lost, +.badge.fault { + background: #ffe9e6; + color: #a54638; +} +#reason { + line-height: 1.6; + color: #7a8b94; + font-size: 12px; + min-height: 38px; + margin: 14px 0 19px; +} +.controls { + display: grid; + gap: 9px; +} +.controls button { + border: 1px solid var(--line); + border-radius: 8px; + background: #fff; + padding: 12px; + font-size: 12px; + font-weight: 650; + color: #375362; +} +.controls button.primary { + background: var(--green); + color: white; + border-color: var(--green); +} +.controls button.stop { + color: #9b514d; + background: #fffafa; + border-color: #f0dedb; +} +.controls button:disabled { + opacity: 0.4; + cursor: default; +} +.controls button:hover:not(:disabled) { + filter: brightness(0.96); +} +.keyhint { + font-size: 10px; + color: #98a5ac; + text-align: center; + margin-top: 14px; +} +kbd { + font: 10px inherit; + border: 1px solid #dce4e8; + border-radius: 4px; + padding: 2px 4px; + background: #f8fafb; +} +.mapping-row { + display: flex; + align-items: center; + justify-content: space-between; + padding: 10px 0; + font-size: 12px; + border-bottom: 1px solid #f0f3f5; +} +.mapping-row:last-child { + border: 0; +} +.axis { + font-family: Consolas, monospace; + color: #637b89; +} +.gain { + font-variant-numeric: tabular-nums; + background: #f2f6f7; + border-radius: 5px; + padding: 4px 7px; + color: #4b636e; +} +.mapping-note { + font-size: 11px; + line-height: 1.6; + color: #95a1a8; + margin: 13px 0 0; +} +.stage { + overflow: hidden; +} +.stage-head { + display: flex; + align-items: center; + justify-content: space-between; + padding: 23px 27px; + border-bottom: 1px solid #ecf0f2; +} +.stage-head span { + font-size: 11px; + color: #82929a; +} +.legend { + display: flex; + gap: 17px; +} +.legend span { + display: flex; + align-items: center; + gap: 6px; +} +.legend i { + width: 7px; + height: 7px; + border-radius: 50%; + background: #1c8a70; +} +.legend .trail { + background: #bdd4cb; +} +.canvas-wrap { + height: 490px; + position: relative; + background: linear-gradient(150deg, #fbfdfd, #f3f7f8); +} +canvas { + width: 100%; + height: 100%; + display: block; + cursor: grab; +} +.view-label { + position: absolute; + top: 22px; + left: 25px; + font-size: 10px; + letter-spacing: 1px; + color: #99a8b0; + pointer-events: none; +} +.view-help { + position: absolute; + bottom: 17px; + right: 23px; + font-size: 10px; + color: #9aa9af; + pointer-events: none; +} +.coordinates { + display: grid; + grid-template-columns: repeat(3, 1fr); + gap: 20px; + padding: 25px 28px; + border-top: 1px solid #e9eff1; +} +.coord-label { + font-size: 10px; + letter-spacing: 1px; + color: #8b9aa4; + margin-bottom: 10px; +} +.coord-value { + font-family: Consolas, monospace; + font-size: 28px; + color: #315365; + letter-spacing: -1px; +} +.coord-value small { + font: + 11px Segoe UI, + sans-serif; + color: #98a7ae; + margin-left: 4px; +} +.bottom { + display: grid; + grid-template-columns: 1fr 1fr; + gap: 20px; + margin-top: 20px; +} +.mini { + padding: 19px 24px; + display: flex; + align-items: center; + justify-content: space-between; + gap: 12px; +} +.mini p { + margin: 5px 0 0; + font-size: 11px; + color: #8b9ba2; +} +.grip-track { + height: 6px; + border-radius: 8px; + background: #e8efed; + width: 115px; + overflow: hidden; +} +.grip-fill { + height: 100%; + background: #288a71; + width: 100%; + transition: width 0.1s; +} +.metric { + font-variant-numeric: tabular-nums; + color: #416675; + font-size: 23px; +} +.footer { + margin-top: 22px; + font-size: 11px; + color: #8b9aa4; + display: flex; + justify-content: space-between; + line-height: 1.7; +} +.message { + color: #ac6650; + font-size: 12px; + min-height: 17px; + margin-top: 12px; +} +#reconnect { + border: 0; + background: none; + color: #477c70; + font-size: 11px; + text-decoration: underline; + padding: 0; + margin-top: 10px; +} +.limit { + color: #b47b24 !important; +} +.empty { + opacity: 0.5; +} +@media (max-width: 1000px) { + main { + padding: 25px 20px; + } + header { + padding: 0 22px; + } + .layout { + grid-template-columns: 255px minmax(300px, 1fr); + } + .section { + padding: 19px; + } + .canvas-wrap { + height: 460px; + } +} +@media (max-width: 720px) { + .layout { + display: flex; + flex-direction: column-reverse; + } + .side { + grid-template-columns: 1fr 1fr; + } + .side .card:last-child { + grid-column: 1/-1; + } + .heading { + align-items: start; + gap: 12px; + } + h1 { + font-size: 27px; + } + .connection { + max-width: 100px; + line-height: 1.8; + } + .canvas-wrap { + height: 340px; + } + .coordinates { + padding: 20px; + gap: 8px; + } + .coord-value { + font-size: 23px; + } + .bottom { + gap: 10px; + } + .mini { + padding: 16px; + } + .grip-track { + width: 65px; + } + .footer { + gap: 20px; + } + .tag { + font-size: 9px; + padding: 8px; + } + .brand { + font-size: 12px; + letter-spacing: 1px; + } + .legend { + gap: 10px; + } +} diff --git a/src/source_robotics_leap_motion/lab.html b/src/source_robotics_leap_motion/lab.html new file mode 100644 index 0000000..856860c --- /dev/null +++ b/src/source_robotics_leap_motion/lab.html @@ -0,0 +1,147 @@ + + + + + + + Teleoperation Lab · Source Robotics + + + +
+
+
↗
+ SOURCE ROBOTICS +
+
SIMULATED ROBOT
+
+
+
+
+
HAND TRACKING / CARTESIAN CONTROL
+

Teleoperation lab

+

Calibrate your reach. Shape the movement.

+
+
+ Connecting +
+
+
+ +
+
+
+

Robot workspace

+
+ Tool targetPath +
+
+
+
CARTESIAN TARGET / ORBIT VIEW
+ +
Drag to orbit · scroll to zoom
+
+
+
+
X POSITION
+
—
+
+
+
Y POSITION
+
—
+
+
+
Z POSITION
+
—
+
+
+
+
+
+
+

Gripper

+

Normalized openness

+
+
+
+
+
+
+
+

Targets delivered

+

Waiting for calibration

+
+ 0 +
+
+
+ Output: simulated Cartesian arm
Feedback follows targets + immediately; no joint or dynamics model.Latest-frame acquisition
Source Robotics · Leap Motion +
+
+
+
+ + + diff --git a/src/source_robotics_leap_motion/lab.js b/src/source_robotics_leap_motion/lab.js new file mode 100644 index 0000000..2285c68 --- /dev/null +++ b/src/source_robotics_leap_motion/lab.js @@ -0,0 +1,344 @@ +const token = document.querySelector("meta[name=local-session]").content, + $ = (id) => document.getElementById(id); +let data = null, + online = false, + busy = false, + requestError = "", + keyClutch = false, + actionChain = Promise.resolve(), + trail = [], + lastSequence = -1; +const reasons = { + not_armed: "Place your hand, then arm to capture a reference.", + armed: "Hand movement is mapped from your calibrated reference.", + clutch_engaged: "Target held. Reposition your hand, then resume.", + waiting_for_reanchor: + "Waiting for a fresh hand frame to capture a new reference.", + clutch_reanchored: "Reference updated. Movement resumes from the held pose.", + hand_missing: "Hand tracking lost. Place your hand and arm again.", + hand_changed: "A different hand was detected. Arm again to calibrate.", + tracking_gap: "Tracking paused. Arm again when frames are fresh.", + stale_frame: "Frames expired. Fresh tracking and rearming are required.", + disconnected: "Controller disconnected. Reconnect tracking, then arm again.", + source_gap: "Source timing changed. Arm again to calibrate.", + source_clock_reset: "Source clock restarted. Arm again to calibrate.", + source_changed: "Tracking source changed. Arm again to calibrate.", + disarmed: "Session disarmed. Arm to capture a fresh reference.", +}; +function action(name) { + actionChain = actionChain.then(async () => { + try { + const r = await fetch("/action", { + method: "POST", + headers: { + "Content-Type": "application/json", + "X-Source-Leap-Token": token, + }, + body: JSON.stringify({ action: name }), + }); + if (!r.ok) throw Error((await r.json()).error); + requestError = ""; + $("message").textContent = ""; + } catch (e) { + requestError = e.message; + $("message").textContent = e.message; + } + }); + return actionChain; +} +function toggleClutch() { + if (data) action(data.state === "clutched" ? "resume" : "clutch"); +} +document.addEventListener("keydown", (e) => { + if (e.code === "Escape") { + action("disarm"); + keyClutch = false; + } + if (e.code === "Space") { + e.preventDefault(); + if (online && !e.repeat && data?.state === "active") { + keyClutch = true; + action("clutch"); + } + } +}); +document.addEventListener("keyup", (e) => { + if (e.code === "Space" && keyClutch) { + e.preventDefault(); + keyClutch = false; + action("resume"); + } +}); +window.addEventListener("blur", () => { + if (keyClutch) { + keyClutch = false; + action("disarm"); + } +}); +async function poll() { + if (busy) return; + busy = true; + try { + const response = await fetch("/state", { cache: "no-store" }); + if (!response.ok) throw Error("Session unavailable"); + const next = await response.json(); + if (!next.robot) return; + data = next; + online = true; + update(); + } catch (e) { + online = false; + $("connection").textContent = "Session ended"; + $("connection-dot").className = "dot"; + $("state").textContent = "OFFLINE"; + $("state").className = "badge"; + $("reason").textContent = + "The lab connection ended. Restart the command and reload this page."; + $("arm").disabled = $("clutch").disabled = $("disarm").disabled = true; + } finally { + busy = false; + } +} +function update() { + const d = data, + t = d.tracking, + c = d.control, + p = d.robot.pose.position; + $("connection").textContent = "Local session"; + $("connection-dot").className = "dot on"; + $("source").textContent = d.source; + $("hand-label").textContent = d.hand + ? `${d.handedness} hand · #${d.hand.id}` + : `No ${d.handedness} hand visible`; + $("fps").textContent = t.received_fps.toFixed(0); + $("age").textContent = + t.last_frame_age_s === null ? "—" : (t.last_frame_age_s * 1000).toFixed(0); + $("state").textContent = d.state.replaceAll("_", " ").toUpperCase(); + $("state").className = "badge " + d.state; + $("reason").textContent = reasons[d.reason] || d.reason.replaceAll("_", " "); + $("message").textContent = d.message || requestError; + $("arm").disabled = + !["disarmed", "tracking_lost"].includes(d.state) || + !d.hand || + t.status !== "streaming"; + $("clutch").disabled = !["active", "clutched"].includes(d.state); + $("clutch").textContent = + d.state === "clutched" + ? "Resume · re-anchor hand" + : "Clutch · reposition hand"; + $("disarm").disabled = ["closed", "fault"].includes(d.state); + for (const a of ["x", "y", "z"]) + $(a).innerHTML = p[a].toFixed(3) + "m"; + $("sequence").textContent = c.sequence; + const limited = c.workspace_limited || c.speed_limited; + $("limit").textContent = c.workspace_limited + ? "Workspace limit applied" + : c.speed_limited + ? "Speed limit applied" + : d.state === "active" + ? "Following calibrated hand" + : "Output held"; + $("limit").className = limited ? "limit" : ""; + $("grip").style.width = (d.robot.gripper ?? 0) * 100 + "%"; + $("grip-label").textContent = + d.robot.gripper === null + ? "Not controlled" + : Math.round(d.robot.gripper * 100) + "% open"; + $("mapping").replaceChildren(); + const rotation = d.config.mapping.rotation; + $("frame-rotation").textContent = + !rotation || + Math.abs(rotation.x) + Math.abs(rotation.y) + Math.abs(rotation.z) < 1e-9 + ? "Frame rotation: identity" + : "Frame rotation (xyzw): " + + [rotation.x, rotation.y, rotation.z, rotation.w] + .map((value) => value.toFixed(3)) + .join(", "); + d.config.mapping.axes.forEach((axis, i) => { + const row = document.createElement("div"); + row.className = "mapping-row"; + const label = document.createElement("span"); + label.className = "axis"; + label.textContent = axis.toUpperCase() + " → " + ["X", "Y", "Z"][i]; + const gain = document.createElement("span"); + gain.className = "gain"; + gain.textContent = Object.values(d.config.mapping.gain)[i].toFixed(2) + "×"; + row.append(label, gain); + $("mapping").append(row); + }); + if (c.sequence !== lastSequence) { + lastSequence = c.sequence; + if (c.sequence === 0) trail = []; + trail.push({ ...p }); + if (trail.length > 240) trail.shift(); + } +} +const canvas = $("scene"), + ctx = canvas.getContext("2d"); +let yaw = -0.75, + pitch = 0.55, + zoom = 1, + drag = null; +canvas.addEventListener("pointerdown", (e) => { + drag = { x: e.clientX, y: e.clientY }; + canvas.setPointerCapture(e.pointerId); +}); +canvas.addEventListener("pointermove", (e) => { + if (drag) { + yaw += (e.clientX - drag.x) * 0.008; + pitch = Math.max(0.15, Math.min(1.3, pitch + (e.clientY - drag.y) * 0.006)); + drag = { x: e.clientX, y: e.clientY }; + } +}); +canvas.addEventListener("pointerup", () => (drag = null)); +canvas.addEventListener("pointercancel", () => (drag = null)); +canvas.addEventListener( + "wheel", + (e) => { + e.preventDefault(); + zoom = Math.max(0.6, Math.min(2, zoom * Math.exp(-e.deltaY * 0.001))); + }, + { passive: false }, +); +function draw() { + const rect = canvas.getBoundingClientRect(), + dpr = window.devicePixelRatio || 1, + w = rect.width, + h = rect.height; + if ( + canvas.width !== Math.round(w * dpr) || + canvas.height !== Math.round(h * dpr) + ) { + canvas.width = Math.round(w * dpr); + canvas.height = Math.round(h * dpr); + } + ctx.setTransform(dpr, 0, 0, dpr, 0, 0); + ctx.clearRect(0, 0, w, h); + if (!data) { + ctx.fillStyle = "#8da0aa"; + ctx.font = "13px Segoe UI"; + ctx.textAlign = "center"; + ctx.fillText("Waiting for the tracking session…", w / 2, h / 2); + requestAnimationFrame(draw); + return; + } + const lo = data.config.workspace.minimum, + hi = data.config.workspace.maximum, + mid = { x: (lo.x + hi.x) / 2, y: (lo.y + hi.y) / 2, z: (lo.z + hi.z) / 2 }, + extent = Math.max(hi.x - lo.x, hi.y - lo.y, hi.z - lo.z, 0.1), + scale = ((Math.min(w, h) * 0.58) / extent) * zoom; + function project(p) { + const x = p.x - mid.x, + y = p.y - mid.y, + z = p.z - mid.z; + const a = Math.cos(yaw) * x - Math.sin(yaw) * y, + b = Math.sin(yaw) * x + Math.cos(yaw) * y; + return [ + w / 2 + a * scale, + h * 0.5 + (Math.sin(pitch) * b - Math.cos(pitch) * z) * scale, + ]; + } + function line(a, b, color, width = 1, dash = []) { + const pa = project(a), + pb = project(b); + ctx.beginPath(); + ctx.strokeStyle = color; + ctx.lineWidth = width; + ctx.setLineDash(dash); + ctx.moveTo(...pa); + ctx.lineTo(...pb); + ctx.stroke(); + ctx.setLineDash([]); + } + for (let i = 0; i <= 10; i++) { + const f = i / 10; + line( + { x: lo.x + (hi.x - lo.x) * f, y: lo.y, z: lo.z }, + { x: lo.x + (hi.x - lo.x) * f, y: hi.y, z: lo.z }, + "#dde7e9", + ); + line( + { x: lo.x, y: lo.y + (hi.y - lo.y) * f, z: lo.z }, + { x: hi.x, y: lo.y + (hi.y - lo.y) * f, z: lo.z }, + "#dde7e9", + ); + } + const corners = []; + for (const x of [lo.x, hi.x]) + for (const y of [lo.y, hi.y]) + for (const z of [lo.z, hi.z]) corners.push({ x, y, z }); + for (let i = 0; i < 8; i++) + for (const bit of [1, 2, 4]) + if ((i ^ bit) > i) + line(corners[i], corners[i ^ bit], "#cad9de", 1, [3, 4]); + for (let i = 1; i < trail.length; i++) + line(trail[i - 1], trail[i], "#9dc9b9", 2); + const p = data.robot.pose.position, + ground = { x: p.x, y: p.y, z: lo.z }; + line(ground, p, "#b7ccc4", 1, [4, 4]); + const [sx, sy] = project(p); + ctx.beginPath(); + ctx.arc(sx, sy, 18, 0, Math.PI * 2); + ctx.fillStyle = online ? "#d5eae380" : "#dde3e780"; + ctx.fill(); + ctx.beginPath(); + ctx.arc(sx, sy, 7, 0, Math.PI * 2); + ctx.fillStyle = + data.state === "active" + ? "#177d67" + : data.state === "clutched" + ? "#ba8a35" + : "#7d969f"; + ctx.fill(); + ctx.strokeStyle = "#fff"; + ctx.lineWidth = 2; + ctx.stroke(); + const length = extent * 0.13, + q = data.robot.pose.orientation; + function rot(v) { + const u = { x: q.x, y: q.y, z: q.z }, + uv = { + x: u.y * v.z - u.z * v.y, + y: u.z * v.x - u.x * v.z, + z: u.x * v.y - u.y * v.x, + }, + uuv = { + x: u.y * uv.z - u.z * uv.y, + y: u.z * uv.x - u.x * uv.z, + z: u.x * uv.y - u.y * uv.x, + }; + return { + x: p.x + v.x + 2 * (q.w * uv.x + uuv.x), + y: p.y + v.y + 2 * (q.w * uv.y + uuv.y), + z: p.z + v.z + 2 * (q.w * uv.z + uuv.z), + }; + } + for (const [v, color] of [ + [{ x: length, y: 0, z: 0 }, "#c8746f"], + [{ x: 0, y: length, z: 0 }, "#5f9e86"], + [{ x: 0, y: 0, z: length }, "#608bb2"], + ]) + line(p, rot(v), color, 2); + const origin = { ...lo }; + for (const [a, color] of [ + ["x", "#b57875"], + ["y", "#5d9681"], + ["z", "#678ba5"], + ]) { + const endpoint = { ...origin, [a]: lo[a] + extent * 0.18 }; + line(origin, endpoint, color, 2); + const screen = project(endpoint); + ctx.fillStyle = color; + ctx.font = "11px Segoe UI"; + ctx.textAlign = "center"; + ctx.fillText(a.toUpperCase(), screen[0], screen[1] - 8); + } + requestAnimationFrame(draw); +} +$("arm").addEventListener("click", () => action("arm")); +$("clutch").addEventListener("click", toggleClutch); +$("disarm").addEventListener("click", () => action("disarm")); +$("reconnect").addEventListener("click", () => action("reconnect")); +poll(); +setInterval(poll, 50); +requestAnimationFrame(draw); diff --git a/src/source_robotics_leap_motion/lab.py b/src/source_robotics_leap_motion/lab.py new file mode 100644 index 0000000..6f09cee --- /dev/null +++ b/src/source_robotics_leap_motion/lab.py @@ -0,0 +1,282 @@ +"""Loopback-only browser lab. Its only output adapter is SimulatedArm.""" + +from __future__ import annotations + +import json +import queue +import secrets +import socket +import sys +import threading +import time +import webbrowser +from contextlib import nullcontext +from dataclasses import asdict +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer +from pathlib import Path + +from .adapters.simulated import SimulatedArm +from .backends.replay import ReplayBackend +from .control_config import GripperMapping, TeleoperationConfig, Workspace +from .control_models import Pose +from .errors import TeleoperationError, TrackingTimeoutError +from .mapping import Mapping +from .models import Vector3 +from .teleoperation import Teleoperator +from .tracker import Tracker + + +class _LabHTTPServer(ThreadingHTTPServer): + # Windows SO_REUSEADDR permits competing listeners. Keep each lab exclusive. + allow_reuse_address = sys.platform != "win32" + + def server_bind(self): + if sys.platform == "win32": + self.socket.setsockopt(socket.SOL_SOCKET, socket.SO_EXCLUSIVEADDRUSE, 1) + super().server_bind() + + +def simulation_config() -> TeleoperationConfig: + return TeleoperationConfig( + workspace=Workspace(Vector3(0, -0.3, 0), Vector3(0.6, 0.3, 0.6)), + max_linear_speed=0.2, + max_angular_speed=1.0, + mapping=Mapping(("-z", "-x", "y"), Vector3(0.75, 0.75, 0.75)), + control_orientation=True, + gripper=GripperMapping(), + ) + + +class LabServer: + def __init__(self, port: int = 0) -> None: + self.commands: queue.Queue[str] = queue.Queue(maxsize=16) + self._lock = threading.Lock() + self._state = b'{"state":"starting"}' + self._token = secrets.token_urlsafe(32) + self._html = ( + Path(__file__) + .with_name("lab.html") + .read_text(encoding="utf-8") + .replace("__TOKEN__", self._token) + .encode("utf-8") + ) + self._assets = { + "/lab.css": ( + Path(__file__).with_name("lab.css").read_bytes(), + "text/css; charset=utf-8", + ), + "/lab.js": ( + Path(__file__).with_name("lab.js").read_bytes(), + "text/javascript; charset=utf-8", + ), + } + lab = self + + class Handler(BaseHTTPRequestHandler): + def log_message(self, *_args): + pass + + def reply(self, status, body, content_type="application/json; charset=utf-8"): + self.send_response(status) + self.send_header("Content-Type", content_type) + self.send_header("Content-Length", str(len(body))) + self.send_header("Cache-Control", "no-store") + self.send_header("X-Content-Type-Options", "nosniff") + self.send_header( + "Content-Security-Policy", + "default-src 'self'; " + "script-src 'self'; style-src 'self'; " + "frame-ancestors 'none'; base-uri 'none'", + ) + self.end_headers() + self.wfile.write(body) + + def do_GET(self): + if self.path == "/": + self.reply(200, lab._html, "text/html; charset=utf-8") + elif self.path == "/state": + with lab._lock: + state = lab._state + self.reply(200, state) + elif self.path in lab._assets: + self.reply(200, *lab._assets[self.path]) + else: + self.reply(404, b'{"error":"not found"}') + + def do_POST(self): + if self.path != "/action": + self.reply(404, b'{"error":"not found"}') + return + if ( + self.headers.get("X-Source-Leap-Token") != lab._token + or self.headers.get("Origin", lab.url) != lab.url + ): + self.reply(403, b'{"error":"invalid local session"}') + return + try: + size = int(self.headers.get("Content-Length", "0")) + if not 0 < size <= 512: + raise ValueError("Invalid body size") + action = json.loads(self.rfile.read(size))["action"] + if action not in ("arm", "clutch", "resume", "disarm", "reconnect"): + raise ValueError("Unknown action") + lab.commands.put_nowait(action) + except queue.Full: + self.reply(429, b'{"error":"action queue is full"}') + return + except (ValueError, KeyError, TypeError): + self.reply(400, b'{"error":"invalid action"}') + return + self.reply(202, b'{"accepted":true}') + + self._server = _LabHTTPServer(("127.0.0.1", port), Handler) + self._server.daemon_threads = True + self._thread = threading.Thread( + target=self._server.serve_forever, name="source-leap-lab", daemon=True + ) + + @property + def url(self) -> str: + return f"http://127.0.0.1:{self._server.server_port}" + + def publish(self, state: dict) -> None: + payload = json.dumps(state, allow_nan=False).encode("utf-8") + with self._lock: + self._state = payload + + def __enter__(self): + self._thread.start() + return self + + def __exit__(self, *_exc): + self._server.shutdown() + self._server.server_close() + self._thread.join(2) + + +def snapshot(teleop: Teleoperator, robot: SimulatedArm, source: str, message: str) -> dict: + status = teleop.status + hand = ( + None + if teleop.last_frame is None + else next( + ( + hand + for hand in teleop.last_frame.hands + if hand.handedness == teleop.config.handedness + ), + None, + ) + ) + return { + "schema": "source-leap.teleoperation-lab", + "version": 1, + "state": status.state, + "reason": status.reason, + "message": message, + "source": source, + "handedness": teleop.config.handedness, + "hand": None if hand is None else {"id": hand.id, "position": asdict(hand.palm.position)}, + "robot": asdict(robot.read_feedback()), + "control": asdict(status), + "tracking": teleop.tracker.health.to_dict(), + "config": asdict(teleop.config), + } + + +def run_lab(args) -> int: + if not 0 <= args.port <= 65535: + raise ValueError("Viewer port must be between 0 and 65535") + if args.json and not args.headless: + raise ValueError("--json requires --headless") + if args.demo and (args.library or args.runtime_dir or args.replay): + raise ValueError("--demo cannot be combined with a live runtime or replay") + if args.replay and (args.library or args.runtime_dir): + raise ValueError("--replay cannot be combined with a live runtime") + config = TeleoperationConfig.from_toml(args.config) if args.config else simulation_config() + if args.demo: + tracker = Tracker.demo(mode="latest", max_frame_age=config.max_frame_age) + source = "synthetic hand" + elif args.replay: + tracker = Tracker( + backend=ReplayBackend(args.replay, realtime=True), + mode="latest", + max_frame_age=config.max_frame_age, + ) + source = "recorded hand" + else: + tracker = Tracker( + mode="latest", + library_path=args.library, + runtime_dir=args.runtime_dir, + max_frame_age=config.max_frame_age, + ) + source = "Leap controller" + center = (config.workspace.minimum + config.workspace.maximum) * 0.5 + robot = SimulatedArm(Pose(center)) + server = None if args.headless else LabServer(args.port) + auto_pending = args.auto_arm + processed = 0 + last_frame = None + message = "" + with ( + server if server is not None else nullcontext(), + tracker, + Teleoperator(tracker, robot=robot, config=config) as teleop, + ): + started = time.monotonic() + if server is not None: + server.publish(snapshot(teleop, robot, source, message)) + print("Teleoperation lab: " + server.url, flush=True) + if not args.no_browser: + webbrowser.open(server.url) + try: + while args.duration is None or time.monotonic() - started < args.duration: + loop_started = time.monotonic() + actions = [] + if server is not None: + while not server.commands.empty(): + actions.append(server.commands.get_nowait()) + if auto_pending and not actions: + actions.append("arm") + for action in actions: + try: + if action == "arm": + teleop.arm(timeout=0.05) + auto_pending = False + elif action == "clutch": + teleop.clutch() + elif action == "resume": + teleop.release_clutch() + elif action == "reconnect": + teleop.reconnect() + else: + auto_pending = False + teleop.disarm() + message = "" + except (TeleoperationError, TrackingTimeoutError) as error: + message = str(error) + timeout = 0.05 + if args.duration is not None: + remaining = args.duration - (time.monotonic() - started) + if remaining <= 0: + break + timeout = min(timeout, remaining) + teleop.step(timeout=timeout) + state = snapshot(teleop, robot, source, message) + if teleop.last_frame is not last_frame: + last_frame = teleop.last_frame + processed += 1 + if args.json: + print(json.dumps(state, allow_nan=False, separators=(",", ":"))) + if server is not None: + server.publish(state) + if args.frames is not None and processed >= args.frames: + break + time.sleep(max(0.0, 1 / config.max_output_hz - (time.monotonic() - loop_started))) + except StopIteration: + pass + print( + f"Simulated targets: {robot.targets_sent}; tracking frames: {processed}.", file=sys.stderr + ) + return 0 diff --git a/src/source_robotics_leap_motion/mapping.py b/src/source_robotics_leap_motion/mapping.py index 45d7ce9..387b2d6 100644 --- a/src/source_robotics_leap_motion/mapping.py +++ b/src/source_robotics_leap_motion/mapping.py @@ -7,7 +7,8 @@ from pathlib import Path from ._compat import tomllib -from .models import Vector3 +from ._geometry import rotate +from .models import Quaternion, Vector3 ZERO = Vector3() @@ -16,14 +17,17 @@ class Mapping: """Map metre positions into an arm's frame using a proper rotation and gains. - target = robot_origin + gain * rotation * (hand_position - hand_origin) + target = robot_origin + gain * rotation * axes * (hand_position - hand_origin) Defaults are identity; no robot-specific bounds or home pose are assumed. """ axes: tuple[str, str, str] = ("x", "y", "z") gain: Vector3 = Vector3(1, 1, 1) + rotation: Quaternion = Quaternion() def __post_init__(self) -> None: + if not isinstance(self.rotation, Quaternion): + raise ValueError("Frame rotation must be a Quaternion") if len(self.axes) != 3 or {axis.lstrip("-") for axis in self.axes} != {"x", "y", "z"}: raise ValueError("Axes must be a signed permutation of x, y, z") if any(axis not in ("x", "y", "z", "-x", "-y", "-z") for axis in self.axes): @@ -48,8 +52,9 @@ def map_position( getattr(delta, axis.lstrip("-")) * (-1 if axis.startswith("-") else 1) for axis in self.axes ] + aligned = rotate(self.rotation, Vector3(*rotated)) return robot_origin + Vector3( - *(component * gain for component, gain in zip(rotated, self.gain, strict=True)) + *(component * gain for component, gain in zip(aligned, self.gain, strict=True)) ) @classmethod @@ -59,10 +64,22 @@ def from_toml(cls, path: str | Path) -> Mapping: if set(document) != {"mapping"}: raise ValueError("Configuration must contain one [mapping] table") config = document["mapping"] - unknown = set(config) - {"axes", "gain"} + unknown = set(config) - {"axes", "gain", "rotation"} if unknown: raise ValueError(f"Unknown mapping settings: {', '.join(sorted(unknown))}") gain = config.get("gain", [1.0, 1.0, 1.0]) if len(gain) != 3: raise ValueError("Gain must have three values") - return cls(tuple(config.get("axes", ("x", "y", "z"))), Vector3(*gain)) + rotation = _parse_rotation(config.get("rotation", [0, 0, 0, 1])) + return cls(tuple(config.get("axes", ("x", "y", "z"))), Vector3(*gain), rotation) + + +def _parse_rotation(values) -> Quaternion: + if not isinstance(values, list) or len(values) != 4: + raise ValueError("Frame rotation requires four x/y/z/w values") + if any( + isinstance(value, bool) or not isinstance(value, (int, float)) or not math.isfinite(value) + for value in values + ): + raise ValueError("Frame rotation requires four finite numbers") + return Quaternion(*values) diff --git a/src/source_robotics_leap_motion/teleoperation.py b/src/source_robotics_leap_motion/teleoperation.py new file mode 100644 index 0000000..72f2422 --- /dev/null +++ b/src/source_robotics_leap_motion/teleoperation.py @@ -0,0 +1,185 @@ +"""Synchronous orchestration of latest-mode tracking and an explicit robot adapter.""" + +from __future__ import annotations + +from ._compat import Self +from .adapters.base import RobotAdapter +from .control import TeleoperationController, TeleoperationStatus +from .control_config import TeleoperationConfig +from .control_models import PoseTarget, RobotFeedback +from .errors import ( + AdapterError, + ConnectionError, + StaleFrameError, + TeleoperationError, + TrackingTimeoutError, +) +from .models import Frame +from .tracker import Tracker + + +class Teleoperator: + """Owns the robot adapter; the caller owns the surrounding Tracker context. + + Call arm explicitly, then step from one application thread. Adapter operations + are synchronous and must be bounded. Controller-side command expiry is required + to handle loss of this application or its network connection. + """ + + def __init__( + self, tracker: Tracker, *, robot: RobotAdapter, config: TeleoperationConfig + ) -> None: + self.tracker, self.robot, self.config = tracker, robot, config + self.controller = TeleoperationController(config) + self._opened = False + self._disconnected = False + self.last_frame: Frame | None = None + + @property + def status(self) -> TeleoperationStatus: + return self.controller.status + + def __enter__(self) -> Self: + if self._opened: + raise TeleoperationError("Teleoperator is already open") + if self.tracker.health.status == "closed" or self.tracker.health.mode != "latest": + raise TeleoperationError("Open a latest-mode Tracker before the Teleoperator") + if self.tracker.max_frame_age > self.config.max_frame_age: + raise TeleoperationError("Tracker max_frame_age must not exceed the control age limit") + capabilities = self.robot.capabilities + if not capabilities.cartesian_position: + raise TeleoperationError( + "Adapter must accept Cartesian positions; supply kinematics there" + ) + if self.config.control_orientation and not capabilities.orientation: + 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") + self.controller = TeleoperationController(self.config) + self._disconnected = False + self.last_frame = None + try: + self._call("open") + self._opened = True + self._call("hold", "disarmed") + except BaseException: + self._opened = False + self.robot.close() + raise + return self + + def _require_open(self) -> None: + if not self._opened: + raise TeleoperationError("Open the Teleoperator with a context manager first") + if self.status.state == "fault": + raise TeleoperationError("Adapter fault is latched; close and reopen before rearming") + + def _call(self, operation: str, *arguments): + try: + return getattr(self.robot, operation)(*arguments) + except Exception as error: + self.controller.fault(f"adapter_{operation}_failed") + extra = "" + if operation in ("send", "read_feedback"): + try: + self.robot.hold("adapter_fault") + except Exception as hold_error: + extra = f"; hold also failed: {hold_error}" + raise AdapterError(f"Adapter {operation} failed: {error}{extra}") from error + + def _reference(self, reference: RobotFeedback | None) -> RobotFeedback: + if reference is None and self.robot.capabilities.feedback: + reference = self._call("read_feedback") + if not isinstance(reference, RobotFeedback): + raise TeleoperationError( + "Provide RobotFeedback explicitly when feedback is unavailable" + ) + return reference + + def arm(self, reference: RobotFeedback | None = None, *, timeout: float | None = None) -> None: + self._require_open() + if self.status.state not in ("disarmed", "tracking_lost"): + raise TeleoperationError("Disarm before calibrating and arming a new session") + if self._disconnected: + raise TeleoperationError("Reconnect tracking before rearming") + reference = self._reference(reference) + frame = self.tracker.read(self.config.tracking_timeout if timeout is None else timeout) + self.last_frame = frame + health = self.tracker.health + 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) + + def disarm(self) -> None: + self._require_open() + self.controller.disarm() + self._call("hold", "disarmed") + + def clutch(self) -> None: + self._require_open() + self.controller.clutch() + self._call("hold", "clutched") + + def release_clutch(self, reference: RobotFeedback | None = None) -> None: + self._require_open() + self.controller.release_clutch(self._reference(reference)) + + def reconnect(self) -> None: + self._require_open() + self.disarm() + self._disconnected = True + self.tracker.reconnect() + self._disconnected = False + + def step(self, *, timeout: float | None = None) -> PoseTarget | None: + self._require_open() + if self._disconnected: + return None + before = self.status.state + try: + frame = self.tracker.read(self.config.tracking_timeout if timeout is None else timeout) + except StaleFrameError: + self.controller.tracking_lost("stale_frame") + frame = None + except TrackingTimeoutError: + frame = None + except ConnectionError: + self.controller.tracking_lost("disconnected") + self._disconnected = True + frame = None + except Exception: + self.controller.fault("tracking_failed") + self._call("hold", "tracking_failed") + raise + if frame is not None: + self.last_frame = frame + health = self.tracker.health + if health.status != "streaming": + reason = "stale_frame" if health.status == "stale" else "tracking_" + health.status + self.controller.tracking_lost(reason) + self._disconnected |= health.status in ("disconnected", "error", "closed") + frame = None + try: + target = self.controller.update(frame, frame_age=health.last_frame_age_s or 0.0) + except Exception: + self.controller.fault("control_failed") + self._call("hold", "control_failed") + raise + if before in ("active", "clutched") and self.status.state == "tracking_lost": + self._call("hold", self.status.reason) + if target is not None: + self._call("send", target) + return target + + def close(self) -> None: + if not self._opened: + return + self._opened = False + self.controller.close() + try: + self._call("hold", "closed") + finally: + self._call("close") + + def __exit__(self, *_exc) -> None: + self.close() diff --git a/src/source_robotics_leap_motion/tracker.py b/src/source_robotics_leap_motion/tracker.py index 76e0cdb..0448807 100644 --- a/src/source_robotics_leap_motion/tracker.py +++ b/src/source_robotics_leap_motion/tracker.py @@ -119,6 +119,11 @@ def __enter__(self) -> Self: self._opened = True return self + @property + def max_frame_age(self) -> float: + """Configured local age limit; enforced on reads in latest mode.""" + return self._max_frame_age + @property def health(self) -> TrackingHealth: """Current session statistics. Reading health never accesses the device.""" diff --git a/tests/test_control.py b/tests/test_control.py new file mode 100644 index 0000000..b066a11 --- /dev/null +++ b/tests/test_control.py @@ -0,0 +1,263 @@ +import math +from dataclasses import replace + +import pytest + +from source_robotics_leap_motion import ( + Frame, + GripperMapping, + Hand, + Mapping, + Palm, + Pose, + Quaternion, + RobotFeedback, + TeleoperationConfig, + TeleoperationController, + TeleoperationError, + Vector3, + Workspace, +) + + +def config(**values): + defaults = dict( + workspace=Workspace(Vector3(-1, -1, -1), Vector3(1, 1, 1)), + max_linear_speed=100, + max_angular_speed=100, + smoothing_time_constant=0, + position_deadband=0, + max_output_hz=1000, + tracking_timeout=0.5, + ) + defaults.update(values) + return TeleoperationConfig(**defaults) + + +def frame( + index=0, + position=None, + *, + orientation=None, + hand_id=1, + hands=True, + timestamp=None, + grab=0, + pinch=0, +): + hand = Hand( + hand_id, + "right", + Palm(position or Vector3(), orientation or Quaternion()), + grab_strength=grab, + pinch_strength=pinch, + ) + return Frame( + index, index * 100_000 if timestamp is None else timestamp, (hand,) if hands else () + ) + + +def armed(**values): + controller = TeleoperationController(config(**values)) + controller.arm(frame(), RobotFeedback(Pose(Vector3()), 1.0), now=0) + return controller + + +def test_disarmed_controller_never_emits_and_arm_does_not_emit(): + controller = TeleoperationController(config()) + assert controller.update(frame(), now=0) is None + controller.arm(frame(), RobotFeedback(Pose(Vector3())), now=0) + assert controller.status.last_target is None + assert controller.update(frame(), now=0.01) is None + assert controller.update(frame(1, Vector3(0.1, 0, 0)), now=0.1).position.x == 0.1 + + +def test_relative_calibration_preserves_robot_pose_and_configured_axes(): + controller = TeleoperationController( + config(mapping=Mapping(("-z", "-x", "y"), Vector3(0.5, 0, 2))) + ) + controller.arm( + frame(position=Vector3(0, 0.3, 0)), RobotFeedback(Pose(Vector3(0.2, 0.1, 0.2))), now=0 + ) + target = controller.update(frame(1, Vector3(0.1, 0.4, -0.2)), now=0.1) + assert tuple(target.position) == pytest.approx((0.3, 0.1, 0.4)) + assert target.orientation is None and target.gripper is None + assert target.sequence == 1 and target.source_frame_id == 1 + + +@pytest.mark.parametrize( + "failure", + [ + "missing", + "hand_changed", + "clock", + "device", + "rewind", + "frozen_clock", + "source_gap", + "local_gap", + "stale", + ], +) +@pytest.mark.parametrize("clutched", [False, True]) +def test_tracking_failures_latch_and_fresh_frames_do_not_rearm(failure, clutched): + controller = armed() + if failure == "rewind": + controller.disarm() + controller.arm(frame(timestamp=200_000), RobotFeedback(Pose(Vector3())), now=0) + if clutched: + controller.clutch() + candidate = frame(1) + now, age = 0.1, 0 + if failure == "missing": + candidate = frame(1, hands=False) + elif failure == "hand_changed": + candidate = frame(1, hand_id=2) + elif failure == "clock": + candidate = replace(candidate, clock_domain="other") + elif failure == "device": + candidate = replace(candidate, device_id=2) + elif failure == "frozen_clock": + candidate = frame(1, timestamp=0) + elif failure == "source_gap": + candidate = frame(1, timestamp=600_000) + elif failure == "local_gap": + now = 0.6 + elif failure == "stale": + age = 0.11 + assert controller.update(candidate, now=now, frame_age=age) is None + assert controller.status.state == "tracking_lost" + assert controller.update(frame(2), now=0.7) is None + assert controller.status.state == "tracking_lost" + + +def test_duplicates_do_not_extend_tracking_deadline_and_explicit_rearm_changes_session(): + controller = armed() + session = controller.status.session_id + for now in (0.1, 0.3, 0.6): + assert controller.update(frame(), now=now) is None + assert controller.status.state == "tracking_lost" + controller.arm(frame(10), RobotFeedback(Pose(Vector3())), now=0.7) + assert controller.status.session_id != session + assert controller.status.sequence == 0 + + +def test_clutch_holds_and_reanchors_without_position_or_rotation_jump(): + controller = armed(control_orientation=True) + first = controller.update(frame(1, Vector3(0.1, 0, 0)), now=0.1) + controller.clutch() + assert controller.update(frame(2, Vector3(0.8, 0, 0)), now=0.2) is None + controller.release_clutch(RobotFeedback(Pose(first.position))) + assert controller.update(frame(3, Vector3(0.9, 0, 0)), now=0.3) is None + assert controller.status.state == "active" + target = controller.update(frame(4, Vector3(1, 0, 0)), now=0.4) + assert target.position.x == pytest.approx(0.2) + assert target.sequence == 2 + + +def test_workspace_and_vector_speed_limit_hold_over_a_long_trajectory(): + bounds = Workspace(Vector3(-0.2, -0.2, -0.2), Vector3(0.2, 0.2, 0.2)) + controller = armed(workspace=bounds, max_linear_speed=0.2) + previous = Vector3() + for index in range(1, 501): + t = index / 100 + target = controller.update( + frame(index, Vector3(math.sin(t) * 2, 2, math.cos(t) * 2), timestamp=index * 10_000), + now=t, + ) + assert bounds.contains(target.position) + assert (target.position - previous).norm <= 0.002 + 1e-12 + previous = target.position + assert controller.status.workspace_limited + + +def test_smoothing_is_time_based_and_deadband_applies_before_smoothing(): + controller = armed(smoothing_time_constant=0.1, position_deadband=0.001) + target = controller.update(frame(1, Vector3(0.01, 0, 0)), now=0.1) + assert target.position.x == pytest.approx(0.01 * (1 - math.exp(-1))) + before = target.position + target = controller.update(frame(2, before + Vector3(0.0005, 0, 0)), now=0.2) + assert target.position == before + + +def test_relative_orientation_is_mapped_in_robot_axes_and_composed_in_world_order(): + root_half = math.sqrt(0.5) + controller = TeleoperationController( + config(control_orientation=True, mapping=Mapping(("-z", "-x", "y"))) + ) + controller.arm( + frame(), RobotFeedback(Pose(Vector3(), Quaternion(0, root_half, 0, root_half))), now=0 + ) + target = controller.update( + frame(1, orientation=Quaternion(0, 0, root_half, root_half)), now=0.1 + ) + q = target.orientation + assert (q.x, q.y, q.z, q.w) == pytest.approx((-0.5, 0.5, -0.5, 0.5)) + + +def test_rotation_gain_and_angular_speed_are_limited(): + controller = armed(control_orientation=True, max_angular_speed=1, rotation_gain=0.5) + target = controller.update(frame(1, orientation=Quaternion(0, 0, 1, 0)), now=0.1) + assert target.orientation.z == pytest.approx(math.sin(0.05)) + assert target.orientation.w == pytest.approx(math.cos(0.05)) + assert controller.status.speed_limited + + +def test_extra_frame_rotation_applies_to_position_and_orientation_together(): + s = math.sqrt(0.5) + controller = armed(control_orientation=True, mapping=Mapping(rotation=Quaternion(0, 0, s, s))) + target = controller.update( + frame(1, Vector3(0.1, 0, 0), orientation=Quaternion(s, 0, 0, s)), now=0.1 + ) + assert tuple(target.position) == pytest.approx((0, 0.1, 0)) + q = target.orientation + assert (q.x, q.y, q.z, q.w) == pytest.approx((0, s, 0, s)) + + +def test_gripper_mapping_limits_speed_and_holds_during_clutch(): + controller = armed(gripper=GripperMapping(source="pinch", max_speed=0.5, deadband=0)) + target = controller.update(frame(1, pinch=1), now=0.1) + assert target.gripper == pytest.approx(0.95) + controller.clutch() + assert controller.update(frame(2, pinch=0), now=0.2) is None + assert controller.status.last_target.gripper == pytest.approx(0.95) + + +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): + assert controller.update(frame(index, timestamp=index * 10_000), now=index / 100) is None + assert controller.update(frame(10, timestamp=100_000), now=0.1).sequence == 1 + + +@pytest.mark.parametrize( + "reference,frame_age", + [ + (RobotFeedback(Pose(Vector3(2, 0, 0))), 0), + (RobotFeedback(Pose(Vector3())), 0.11), + ], +) +def test_invalid_calibration_stays_disarmed(reference, frame_age): + controller = TeleoperationController(config()) + with pytest.raises(TeleoperationError): + controller.arm(frame(), reference, now=0, frame_age=frame_age) + assert controller.status.state == "disarmed" + + +def test_gripper_requires_known_reference_and_missing_hand_cannot_calibrate(): + controller = TeleoperationController(config(gripper=GripperMapping())) + with pytest.raises(TeleoperationError, match="gripper reference"): + controller.arm(frame(), RobotFeedback(Pose(Vector3())), now=0) + with pytest.raises(TeleoperationError, match="exactly one"): + controller.arm(frame(hands=False), RobotFeedback(Pose(Vector3()), 1), now=0) + + +def test_local_clock_regression_faults_and_cannot_rearm_without_new_controller(): + controller = armed() + controller.update(frame(1), now=0.1) + with pytest.raises(TeleoperationError, match="backwards"): + controller.update(frame(2), now=0.05) + assert controller.status.state == "fault" + controller.disarm() + with pytest.raises(TeleoperationError): + controller.arm(frame(2), RobotFeedback(Pose(Vector3())), now=0.2) diff --git a/tests/test_control_config.py b/tests/test_control_config.py new file mode 100644 index 0000000..7d6fc29 --- /dev/null +++ b/tests/test_control_config.py @@ -0,0 +1,62 @@ +import pytest + +from source_robotics_leap_motion import GripperMapping, TeleoperationConfig, Vector3, Workspace + + +@pytest.mark.parametrize( + "values", + [ + {"max_linear_speed": 0}, + {"max_linear_speed": float("nan")}, + {"max_linear_speed": True}, + {"smoothing_time_constant": -1}, + {"position_deadband": -1}, + {"max_frame_age": 0.2}, + {"max_output_hz": 1001}, + {"handedness": "both"}, + {"control_orientation": "true"}, + ], +) +def test_invalid_settings_are_rejected(values): + settings = {"workspace": Workspace(Vector3(), Vector3(1, 1, 1)), "max_linear_speed": 1} + settings.update(values) + with pytest.raises(ValueError): + TeleoperationConfig(**settings) + + +def test_workspace_rejects_inverted_bounds(): + with pytest.raises(ValueError, match="minimum"): + Workspace(Vector3(1, 0, 0), Vector3()) + + +@pytest.mark.parametrize( + "settings", + [{"source": "fist"}, {"max_speed": 0}, {"open_value": 1.1}, {"closed_value": float("nan")}], +) +def test_invalid_gripper_settings(settings): + with pytest.raises(ValueError): + GripperMapping(**settings) + + +def test_toml_configuration_and_typo_rejection(tmp_path): + path = tmp_path / "arm.toml" + text = """[workspace] +minimum = [0, -0.3, 0] +maximum = [0.6, 0.3, 0.6] +[mapping] +axes = ["-z", "-x", "y"] +gain = [0.5, 0.5, 0.5] +[teleoperation] +max_linear_speed = 0.2 +control_orientation = true +[gripper] +source = "pinch" +""" + 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 + 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) diff --git a/tests/test_lab.py b/tests/test_lab.py new file mode 100644 index 0000000..7c6af7a --- /dev/null +++ b/tests/test_lab.py @@ -0,0 +1,88 @@ +import json +import urllib.error +import urllib.request +from urllib.parse import urlsplit + +import pytest + +from source_robotics_leap_motion.cli import main +from source_robotics_leap_motion.lab import LabServer + + +def test_headless_demo_starts_disarmed_and_never_sends_without_activation(capsys): + assert main(["teleop", "--demo", "--headless", "--json", "--frames", "3"]) == 0 + captured = capsys.readouterr() + states = [json.loads(line) for line in captured.out.splitlines()] + assert len(states) == 3 + assert all(state["state"] == "disarmed" for state in states) + assert all(state["control"]["sequence"] == 0 for state in states) + assert "Simulated targets: 0" in captured.err + + +def test_headless_demo_can_explicitly_arm_simulation(capsys): + assert main(["teleop", "--demo", "--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]["state"] == "active" + assert states[-1]["control"]["sequence"] > 0 + + +def test_lab_server_serves_packaged_viewer_and_authenticated_bounded_actions(): + server = LabServer() + with server: + server.publish({"state": "disarmed"}) + with urllib.request.urlopen(server.url, timeout=3) as response: + html = response.read().decode() + assert "Teleoperation lab" in html + assert "__TOKEN__" not in html + assert "frame-ancestors 'none'" in response.headers["Content-Security-Policy"] + with urllib.request.urlopen(server.url + "/state", timeout=3) as response: + assert json.load(response) == {"state": "disarmed"} + for asset, content_type in [("lab.js", "text/javascript"), ("lab.css", "text/css")]: + with urllib.request.urlopen(server.url + "/" + asset, timeout=3) as response: + assert response.headers["Content-Type"].startswith(content_type) + assert len(response.read()) > 100 + + def action(token, origin, body=b'{"action":"arm"}'): + request = urllib.request.Request( + server.url + "/action", + data=body, + headers={"X-Source-Leap-Token": token, "Origin": origin}, + method="POST", + ) + return urllib.request.urlopen(request, timeout=3) + + for token, origin in [("wrong", server.url), (server._token, "https://example.com")]: + with pytest.raises(urllib.error.HTTPError) as caught: + action(token, origin) + assert caught.value.code == 403 + with action(server._token, server.url) as response: + assert response.status == 202 + assert server.commands.get_nowait() == "arm" + with pytest.raises(urllib.error.HTTPError) as caught: + action(server._token, server.url, b'{"action":"arbitrary_robot_command"}') + assert caught.value.code == 400 + for _ in range(16): + server.commands.put_nowait("arm") + with pytest.raises(urllib.error.HTTPError) as caught: + action(server._token, server.url) + assert caught.value.code == 429 + assert not server._thread.is_alive() + + +def test_lab_port_cannot_be_shared_by_two_simulator_processes(): + with LabServer() as server: + with pytest.raises(OSError): + LabServer(urlsplit(server.url).port) + + +@pytest.mark.parametrize( + "arguments", + [ + ["--json"], + ["--demo", "--replay", "anything.jsonl"], + ["--port", "-1"], + ], +) +def test_invalid_lab_options_report_errors(arguments, capsys): + assert main(["teleop", *arguments]) == 2 diff --git a/tests/test_mapping.py b/tests/test_mapping.py index 5144aa2..5305658 100644 --- a/tests/test_mapping.py +++ b/tests/test_mapping.py @@ -1,6 +1,24 @@ +import math + import pytest -from source_robotics_leap_motion import Mapping, Vector3 +from source_robotics_leap_motion import Mapping, Quaternion, Vector3 + + +def test_arbitrary_frame_rotation_precedes_robot_axis_gains(): + # Rotate 45 degrees around z, then scale the robot x axis by two. + mapping = Mapping( + rotation=Quaternion(0, 0, math.sin(math.pi / 8), math.cos(math.pi / 8)), + gain=Vector3(2, 1, 1), + ) + point = mapping.map_position(Vector3(1, 0, 0)) + assert tuple(point) == pytest.approx((math.sqrt(2), math.sqrt(0.5), 0)) + + +def test_mapping_toml_supports_explicit_quaternion_rotation(tmp_path): + path = tmp_path / "rotation.toml" + path.write_text("[mapping]\nrotation = [0.0, 0.0, 1.0, 0.0]\n", encoding="utf-8") + assert Mapping.from_toml(path).map_position(Vector3(1, 0, 0)).x == pytest.approx(-1) def test_mapping_preserves_calibrated_robot_origin_and_relative_axes(): diff --git a/tests/test_teleoperation.py b/tests/test_teleoperation.py new file mode 100644 index 0000000..0437440 --- /dev/null +++ b/tests/test_teleoperation.py @@ -0,0 +1,240 @@ +from collections import deque +from types import SimpleNamespace + +import pytest + +from source_robotics_leap_motion import ( + AdapterError, + ConnectionError, + Frame, + Hand, + Palm, + Pose, + RobotCapabilities, + RobotFeedback, + StaleFrameError, + TeleoperationConfig, + TeleoperationError, + Teleoperator, + TrackingTimeoutError, + Vector3, + Workspace, +) +from source_robotics_leap_motion.adapters import CallbackAdapter, SimulatedArm + + +class Source: + max_frame_age = 0.05 + + def __init__(self): + self.health = SimpleNamespace(status="streaming", mode="latest", last_frame_age_s=0) + self.now = 0 + self.events = deque() + self.index = 0 + self.reconnects = 0 + + def read(self, timeout): + self.now += 0.05 + if self.events: + event = self.events.popleft() + if isinstance(event, BaseException): + raise event + return event + self.index += 1 + return Frame( + self.index, + self.index * 50_000, + (Hand(1, "right", Palm(Vector3(self.index / 100, 0, 0))),), + ) + + def reconnect(self): + self.reconnects += 1 + self.health.status = "streaming" + + +@pytest.fixture +def setup(monkeypatch): + source = Source() + monkeypatch.setattr("source_robotics_leap_motion.control.time.monotonic", lambda: source.now) + config = TeleoperationConfig( + Workspace(Vector3(-1, -1, -1), Vector3(1, 1, 1)), + max_linear_speed=1, + smoothing_time_constant=0, + position_deadband=0, + ) + robot = SimulatedArm(Pose(Vector3())) + return source, config, robot + + +def test_managed_session_starts_disarmed_owns_adapter_and_keeps_tracker_open(setup): + source, config, robot = setup + manager = Teleoperator(source, robot=robot, config=config) + with manager: + assert robot.hold_reason == "disarmed" + assert manager.step() is None and robot.targets_sent == 0 + manager.arm() + assert robot.targets_sent == 0 + assert manager.step() is robot.last_target + manager.clutch() + assert robot.hold_reason == "clutched" + before = robot.targets_sent + assert manager.step() is None + assert robot.targets_sent == before + manager.release_clutch() + assert manager.step() is None + assert manager.step() is not None + assert manager.status.state == "closed" + assert source.health.status == "streaming" + with pytest.raises(AdapterError): + robot.read_feedback() + manager.close() + + +@pytest.mark.parametrize( + "event", [StaleFrameError("stale"), ConnectionError("unplugged"), Frame(200, 150_000)] +) +def test_tracking_loss_sends_hold_once_and_requires_explicit_rearm(setup, event): + source, config, robot = setup + holds = [] + callback = CallbackAdapter( + lambda target: None, hold=holds.append, feedback=lambda: RobotFeedback(Pose(Vector3())) + ) + with Teleoperator(source, robot=callback, config=config) as manager: + manager.arm() + source.events.append(event) + assert manager.step() is None + assert manager.status.state == "tracking_lost" + assert len(holds) == 2 # Startup hold and one loss transition. + manager.step() + assert len(holds) == 2 + if isinstance(event, ConnectionError): + with pytest.raises(TeleoperationError, match="Reconnect"): + manager.arm() + manager.reconnect() + assert source.reconnects == 1 + manager.arm() + assert manager.status.state == "active" + + +def test_short_poll_timeout_does_not_disarm_but_expired_tracking_does(setup): + source, config, robot = setup + with Teleoperator(source, robot=robot, config=config) as manager: + manager.arm() + source.events.append(TrackingTimeoutError("poll")) + assert manager.step(timeout=0.01) is None + assert manager.status.state == "active" + source.now += 0.6 + source.events.append(TrackingTimeoutError("poll")) + manager.step() + assert manager.status.state == "tracking_lost" + assert robot.hold_reason == "tracking_gap" + + +def test_callback_without_feedback_requires_an_explicit_reference(setup): + source, config, _robot = setup + commands = [] + callback = CallbackAdapter(commands.append, hold=lambda reason: None) + with Teleoperator(source, robot=callback, config=config) as manager: + with pytest.raises(TeleoperationError, match="RobotFeedback"): + manager.arm() + manager.arm(RobotFeedback(Pose(Vector3(0.3, 0, 0)))) + manager.step() + assert commands[0].position.x == pytest.approx(0.31) + + +@pytest.mark.parametrize("operation", ["send", "read_feedback"]) +def test_adapter_errors_latch_fault_preserve_cause_and_attempt_hold(setup, operation): + source, config, robot = setup + original = getattr(robot, operation) + + def fail(*args): + raise OSError("Controller offline") + + with Teleoperator(source, robot=robot, config=config) as manager: + manager.arm() + setattr(robot, operation, fail) + with pytest.raises(AdapterError, match="Controller offline") as caught: + manager.step() if operation == "send" else manager._reference(None) + assert isinstance(caught.value.__cause__, OSError) + assert manager.status.state == "fault" + assert robot.hold_reason == "adapter_fault" + with pytest.raises(TeleoperationError, match="fault"): + manager.arm() + setattr(robot, operation, original) + + +def test_invalid_tracking_packet_holds_before_propagating_error(setup): + source, config, robot = setup + with Teleoperator(source, robot=robot, config=config) as manager: + manager.arm() + source.events.append(ValueError("Bad packet")) + with pytest.raises(ValueError, match="Bad packet"): + manager.step() + assert manager.status.state == "fault" + assert robot.hold_reason == "tracking_failed" + + +def test_health_failure_observed_after_read_prevents_target_delivery(setup): + source, config, robot = setup + with Teleoperator(source, robot=robot, config=config) as manager: + manager.arm() + source.health.status = "disconnected" + assert manager.step() is None + assert robot.targets_sent == 0 + assert manager.status.state == "tracking_lost" + assert robot.hold_reason == "tracking_disconnected" + + +@pytest.mark.parametrize( + "mode,age,status", + [("sequential", 0.05, "streaming"), ("latest", 0.2, "streaming"), ("latest", 0.05, "closed")], +) +def test_incompatible_tracking_is_rejected_before_robot_open(setup, mode, age, status): + source, config, robot = setup + source.health.mode, source.health.status, source.max_frame_age = mode, status, age + with pytest.raises(TeleoperationError): + with Teleoperator(source, robot=robot, config=config): + pass + assert not robot._opened + + +def test_unsupported_adapter_capabilities_fail_before_open(setup): + source, config, robot = setup + robot.capabilities = RobotCapabilities(cartesian_position=False) + with pytest.raises(TeleoperationError, match="kinematics"): + with Teleoperator(source, robot=robot, config=config): + pass + assert not robot._opened + + +def test_partial_callback_startup_and_failed_hold_still_release_resources(setup): + source, config, _robot = setup + closed = [] + + def failed_open(): + raise OSError("Startup failed") + + callback = CallbackAdapter( + lambda target: None, + hold=lambda reason: None, + open=failed_open, + close=lambda: closed.append(True), + ) + with pytest.raises(AdapterError, match="Startup failed"): + with Teleoperator(source, robot=callback, config=config): + pass + assert closed == [True] + + +def test_close_releases_adapter_even_when_hold_fails(setup): + source, config, robot = setup + manager = Teleoperator(source, robot=robot, config=config) + manager.__enter__() + + def fail(reason): + raise OSError("Hold failed") + + robot.hold = fail + with pytest.raises(AdapterError, match="Hold failed"): + manager.close() + assert not robot._opened