diff --git a/README.md b/README.md index 5647d77..2856b77 100644 --- a/README.md +++ b/README.md @@ -5,116 +5,97 @@ A teleoperation system for OpenArm robots using KER (Kinematic Equivalent Replic ## Features - **Joint mapping**: Flexible configuration-based mapping from leader to follower joints -- **Serial communication**: Interface with M5Stack CoreS3 over UART +- **USB communication**: Interface with M5Stack CoreS3 via USB vendor mode -## Quick start +## Quick Start -### Install +### 1. Install system dependencies ```bash -pip install openarm_ker +sudo apt install libusb-1.0-0-dev ``` -### Serial device permissions +### 2. Set up udev rules (run once) -On Linux, serial devices such as `/dev/ttyACM0` are usually owned by the -`dialout` group. Add your user to that group, then log out and log back in. -If you run the examples from VS Code or another terminal, restart that program -so it picks up the new group permissions. +**USB vendor mode only (normal use):** ```bash -sudo usermod -aG dialout "$USER" +echo 'SUBSYSTEM=="usb", ATTRS{idVendor}=="303a", MODE="0666"' | sudo tee /etc/udev/rules.d/99-m5stack.rules +sudo udevadm control --reload-rules && sudo udevadm trigger ``` -For a temporary test, you can also relax the permission of the current device -node directly: +**If you also want to flash firmware (adds stable device name `/dev/m5_ker_485`):** + +Put M5Stack into flashing mode (hold RST 3 seconds until green LED lights up), then run: ```bash -sudo chmod 666 /dev/ttyACM0 +SERIAL=$(udevadm info -q property -n /dev/ttyACM0 | grep ID_SERIAL_SHORT | cut -d= -f2) +sudo tee /etc/udev/rules.d/99-m5stack.rules << EOF +# USB vendor mode (normal operation) +SUBSYSTEM=="usb", ATTRS{idVendor}=="303a", MODE="0666" + +# Serial mode (flashing) with stable device name +SUBSYSTEM=="tty", ATTRS{idVendor}=="303a", ATTRS{idProduct}=="1001", ATTRS{serial}=="$SERIAL", MODE="0666", SYMLINK+="m5_ker_485" +EOF +sudo udevadm control --reload-rules && sudo udevadm trigger ``` -This usually resets after the device is unplugged or the device node is -recreated. Use this only as a short-lived local test because it makes the device -writable by every local user. For regular use, prefer the `dialout` group or a -udev rule with `MODE="0660"`. +Press RST once to reboot normally. -To use a stable device name such as `/dev/m5_ker_485`, create a udev rule. -First inspect the device properties: +### 3. Install ```bash -udevadm info -q property -n /dev/ttyACM0 +uv pip install openarm_ker ``` -Record fields like these: - -```bash -ID_VENDOR_ID=xxxx -ID_MODEL_ID=yyyy -ID_SERIAL_SHORT=zzzz -``` +### 4. Connect M5Stack and verify -Create a rule file: +Plug the M5Stack CoreS3 into your PC via USB and run: ```bash -sudo nano /etc/udev/rules.d/99-openarm-ker.rules +openarm-ker-cli ping ``` -Add a rule like this, replacing `xxxx`, `yyyy`, and `zzzz` with your device's -actual values: +Expected output: -```udev -SUBSYSTEM=="tty", ENV{ID_VENDOR_ID}=="xxxx", ENV{ID_MODEL_ID}=="yyyy", ENV{ID_SERIAL_SHORT}=="zzzz", SYMLINK+="m5_ker_485", GROUP="dialout", MODE="0660" +```json +{ + "fw": "v1.0.0", + "hw": "KER-v1.0.0", + "updated": "2026-05-25" +} ``` -If you do not need to distinguish between multiple devices of the same model, -you can omit `ENV{ID_SERIAL_SHORT}=="zzzz"`. For a personal development machine, -you can instead use `MODE="0666"` and omit `GROUP` to allow all local users to -access it. +### 5. Sample usage -Reload the rules: - -```bash -sudo udevadm control --reload-rules -sudo udevadm trigger +```python +from openarm_ker import KERStream + +with KERStream(transport="usb") as stream: + data = stream.latest() + if data is not None: + ts = data["timestamp"] + angles = data["angles"] + enc_val = data["encoder_value"] + enc_btn = data["encoder_button"] + angles_str = " | ".join([f"CH{i+1:02d}: {a:8.2f}°" for i, a in enumerate(angles)]) + print(f"TS: {ts:10d} | {angles_str} | ENC: {enc_val:4d} (Btn: {int(enc_btn)})", end='\r') ``` -Then reconnect the device and check that the stable device name was created: +## CLI ```bash -ls -l /dev/m5_ker_485 -``` +# Check device connection and fetch schema +openarm-ker-cli ping -You can then use `/dev/m5_ker_485` as the serial device path. +# Stream raw data to terminal +openarm-ker-cli stream -### Sample usage - -```python -import numpy as np -import openarm_ker - -m5_port = openarm_ker.M5Port("/dev/ttyACM0") - -leader_joint_names = [f"right_arm_joint{i}" for i in range(1, 9)] -mapper = openarm_ker.Mapper( - mappingyaml_path="mapping_m5.yaml", - leader_joint_names=leader_joint_names, - mapping_key="right_arm_mappings", -) - -m5_port.fetch_present_status_bulk() -leader_position = m5_port.present_position -follower_position = mapper.map(np.deg2rad(leader_position)) +# Serial transport +openarm-ker-cli stream --transport serial --port /dev/m5_ker_485 --baud 2000000 ``` -### Mapper config - -The main M5 mapping file is `src/openarm_ker/config/mapping_m5.yaml` in this -repository. It is bundled in the installed package under `openarm_ker/config/`, -so you can pass the bundled filename `mapping_m5.yaml`, or pass an explicit path -to a custom YAML file. For the left arm, use `left_arm_joint*` leader names with -`mapping_key="left_arm_mappings"`. - -## Related links +## Related Links - 📚 Read the [documentation](https://docs.openarm.dev/software/can/) - 💬 Join the community on [Discord](https://discord.gg/FsZaZ4z3We) diff --git a/pyproject.toml b/pyproject.toml index 05f257a..9906fd4 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -23,15 +23,19 @@ version = "0.1.0" authors = [{name = "Enactic, Inc."}] license = {text = "Apache-2.0"} readme = "README.md" -requires-python = ">=3.10" +requires-python = ">=3.11" dependencies = [ "numpy>=1.20.0", "PyYAML>=5.4.0", + "pyusb", "pyserial", "openarm-can>=0.1.0", ] +[project.scripts] +openarm-ker-cli = "openarm_ker.cli:main" + [project.urls] changelog = "https://github.com/enactic/openarm_ker/releases" issues = "https://github.com/enactic/openarm_ker/issues" diff --git a/src/openarm_ker/__init__.py b/src/openarm_ker/__init__.py index 91ff6b1..a15e0f2 100644 --- a/src/openarm_ker/__init__.py +++ b/src/openarm_ker/__init__.py @@ -14,10 +14,6 @@ """OpenArm KER - Teleoperation system for OpenArm robots.""" -from .m5_port import ( - M5Port as M5Port, -) - -from .mapper import ( - Mapper as Mapper, +from .ker_stream import ( + KERStream as KERStream, ) diff --git a/src/openarm_ker/cli.py b/src/openarm_ker/cli.py new file mode 100644 index 0000000..137a7df --- /dev/null +++ b/src/openarm_ker/cli.py @@ -0,0 +1,98 @@ +# Copyright 2026 Enactic, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Command-line interface utilities for OpenArm KER.""" + +import argparse +import json +import sys +import time +from typing import NoReturn + +from .ker_stream import KERStream + + +def main() -> NoReturn | None: + """Run the KER CLI. + + Provides diagnostic utilities such as pinging the device and raw streaming. + """ + parser = argparse.ArgumentParser( + description="KERStream Command-Line Interface (CLI) Utility", + formatter_class=argparse.ArgumentDefaultsHelpFormatter, + ) + parser.add_argument( + "command", + choices=["ping", "stream"], + help="Command to execute: 'ping' to fetch schema and device metadata, 'stream' to test continuous data reception.", + ) + parser.add_argument( + "--transport", + type=str, + default="usb", + choices=["usb", "serial"], + help="Transport protocol connection type.", + ) + parser.add_argument( + "--port", + type=str, + default="/dev/ttyACM0", + help="Serial port path (only applicable when transport is set to 'serial').", + ) + parser.add_argument( + "--baud", + type=int, + default=2000000, + help="Baud rate speed (only applicable when transport is set to 'serial').", + ) + args = parser.parse_args() + + stream = KERStream(transport=args.transport, port=args.port, baud=args.baud) + + if args.command == "ping": + metadata = stream.ping_only() + if metadata: + print(json.dumps(metadata, indent=2)) + sys.exit(0) + else: + print( + "Error: Failed to fetch metadata or no response from the device.", + file=sys.stderr, + ) + sys.exit(1) + + elif args.command == "stream": + print(f"[Info] Starting data stream via {args.transport.upper()}...") + print("[Info] Press Ctrl+C to terminate the stream safely.\n") + try: + with stream: + while stream.is_connected: + data = stream.latest() + if data: + print(f"\r[Stream Data] {data}", end="", flush=True) + time.sleep(0.01) + print("\n[Warning] Stream loop terminated. Device connection lost.") + except KeyboardInterrupt: + print("\n[Info] Stream terminated by user. Cleaning up resources.") + sys.exit(0) + except Exception as e: + print( + f"\n[Critical] Stream crashed with an unexpected error: {e}", + file=sys.stderr, + ) + sys.exit(1) + + +if __name__ == "__main__": + main() diff --git a/src/openarm_ker/config/mapping_m5.yaml b/src/openarm_ker/config/mapping_m5.yaml deleted file mode 100644 index 33b595e..0000000 --- a/src/openarm_ker/config/mapping_m5.yaml +++ /dev/null @@ -1,147 +0,0 @@ -# Copyright 2026 Enactic, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -right_arm_mappings: - # --- Joint 1 --- - - leader: right_arm_joint1 - follower: openarm_right_joint1 - sign: 1.0 - scale: 1.0 - offset: -0.95993 - mech_limits: [-1.3, 3.48888] - - # --- Joint 2 --- - - leader: right_arm_joint2 - follower: openarm_right_joint2 - sign: 1.0 - scale: 1.0 - offset: -0.349065 - mech_limits: [-0.174533, 3.316125] - - # --- Joint 3 --- - - leader: right_arm_joint3 - follower: openarm_right_joint3 - sign: 1.0 - scale: 1.0 - offset: 1.18944 - mech_limits: [-1.570796, 1.570796] - - # --- Joint 4 --- - - leader: right_arm_joint4 - follower: openarm_right_joint4 - sign: 1.0 - scale: 1.0 - offset: -2.12232 - mech_limits: [0.0, 2.443461] - - # --- Joint 5 --- - - leader: right_arm_joint5 - follower: openarm_right_joint5 - sign: 1.0 - scale: 1.0 - offset: -1.128355 - mech_limits: [-1.570796, 1.570796] - - # --- Joint 6 --- - - leader: right_arm_joint6 - follower: openarm_right_joint6 - sign: 1.0 - scale: 1.0 - offset: -0.397411 - mech_limits: [-0.785398, 0.785398] - - # --- Joint 7 --- - - leader: right_arm_joint7 - follower: openarm_right_joint7 - sign: 1.0 - scale: 1.0 - offset: -1.571 - mech_limits: [-1.570796, 1.570796] - - # --- Gripper (Joint 8) --- - - leader: right_arm_joint8 - follower: openarm_right_gripper - sign: 1.0 - scale: 1.0 - offset: 0.0 - open_range: [-0.8, 0.1] - mech_limits: [-1.5707996, 0.1] - leader_range: [0.1, -0.96] - -left_arm_mappings: - # --- Joint 1 --- - - leader: left_arm_joint1 - follower: openarm_left_joint1 - sign: 1.0 - scale: 1.0 - offset: 0.95993 - mech_limits: [-3.48888, 1.396263] - - # --- Joint 2 --- - - leader: left_arm_joint2 - follower: openarm_left_joint2 - sign: 1.0 - scale: 1.0 - offset: 0.349065 - mech_limits: [-3.316125, 0.174533] - - # --- Joint 3 --- - - leader: left_arm_joint3 - follower: openarm_left_joint3 - sign: 1.0 - scale: 1.0 - offset: -1.18944 - mech_limits: [-1.570796, 1.570796] - - # --- Joint 4 --- - - leader: left_arm_joint4 - follower: openarm_left_joint4 - sign: 1.0 - scale: 1.0 - offset: -2.12232 - mech_limits: [0.0, 2.443461] - - # --- Joint 5 --- - - leader: left_arm_joint5 - follower: openarm_left_joint5 - sign: 1.0 - scale: 1.0 - offset: 1.128355 - mech_limits: [-1.570796, 1.570796] - - # --- Joint 6 --- - - leader: left_arm_joint6 - follower: openarm_left_joint6 - sign: 1.0 - scale: 1.0 - offset: 0.397411 - mech_limits: [-0.785398, 0.785398] - - # --- Joint 7 --- - - leader: left_arm_joint7 - follower: openarm_left_joint7 - sign: 1.0 - scale: 1.0 - offset: 1.571 - mech_limits: [-1.570796, 1.570796] - - # --- Gripper (Joint 8) --- - - leader: left_arm_joint8 - follower: openarm_left_gripper - sign: 1.0 - scale: 1.0 - offset: 0.0 - mech_limits: [-0.1, 1.5707996] # follower - open_range: [0.8, -0.2] # follower open range - leader_range: [0.0, 0.96] # leader open range diff --git a/src/openarm_ker/config/mapping_m5_left.yaml b/src/openarm_ker/config/mapping_m5_left.yaml deleted file mode 100644 index 8edc4e4..0000000 --- a/src/openarm_ker/config/mapping_m5_left.yaml +++ /dev/null @@ -1,80 +0,0 @@ -# Copyright 2026 Enactic, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -left_arm_mappings: - # --- Joint 1 --- - - leader: arm_joint1 - follower: openarm_joint1 - sign: 1.0 - scale: 1.0 - offset: 0.95993 - mech_limits: [-3.48888, 1.396263] - - # --- Joint 2 --- - - leader: arm_joint2 - follower: openarm_joint2 - sign: 1.0 - scale: 1.0 - offset: 0.349065 - mech_limits: [-3.316125, 0.174533] - - # --- Joint 3 --- - - leader: arm_joint3 - follower: openarm_joint3 - sign: 1.0 - scale: 1.0 - offset: -1.18944 - mech_limits: [-1.570796, 1.570796] - - # --- Joint 4 --- - - leader: arm_joint4 - follower: openarm_joint4 - sign: 1.0 - scale: 1.0 - offset: -2.12232 - mech_limits: [0.0, 2.443461] - - # --- Joint 5 --- - - leader: arm_joint5 - follower: openarm_joint5 - sign: 1.0 - scale: 1.0 - offset: 1.128355 - mech_limits: [-1.570796, 1.570796] - - # --- Joint 6 --- - - leader: arm_joint6 - follower: openarm_joint6 - sign: 1.0 - scale: 1.0 - offset: 0.397411 - mech_limits: [-0.785398, 0.785398] - - # --- Joint 7 --- - - leader: arm_joint7 - follower: openarm_joint7 - sign: 1.0 - scale: 1.0 - offset: 1.571 - mech_limits: [-1.570796, 1.570796] - - # --- Joint 8 --- - - leader: arm_joint8 - follower: openarm_gripper - sign: 1.0 - scale: 1.0 - offset: 0.0 - open_range: [0.8, -0.3] - mech_limits: [-0.1, 1.5707996] - leader_range: [0.0, 0.96] \ No newline at end of file diff --git a/src/openarm_ker/config/mapping_m5_right.yaml b/src/openarm_ker/config/mapping_m5_right.yaml deleted file mode 100644 index 61dcf34..0000000 --- a/src/openarm_ker/config/mapping_m5_right.yaml +++ /dev/null @@ -1,80 +0,0 @@ -# Copyright 2026 Enactic, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -right_arm_mappings: - # --- Joint 1 --- - - leader: arm_joint1 - follower: openarm_joint1 - sign: 1.0 - scale: 1.0 - offset: -0.95993 - mech_limits: [-1.3, 3.48888] - - # --- Joint 2 --- - - leader: arm_joint2 - follower: openarm_joint2 - sign: 1.0 - scale: 1.0 - offset: -0.349065 - mech_limits: [-0.174533, 3.316125] - - # --- Joint 3 --- - - leader: arm_joint3 - follower: openarm_joint3 - sign: 1.0 - scale: 1.0 - offset: 1.18944 - mech_limits: [-1.570796, 1.570796] - - # --- Joint 4 --- - - leader: arm_joint4 - follower: openarm_joint4 - sign: 1.0 - scale: 1.0 - offset: -2.12232 - mech_limits: [0.0, 2.443461] - - # --- Joint 5 --- - - leader: arm_joint5 - follower: openarm_joint5 - sign: 1.0 - scale: 1.0 - offset: -1.128355 - mech_limits: [-1.570796, 1.570796] - - # --- Joint 6 --- - - leader: arm_joint6 - follower: openarm_joint6 - sign: 1.0 - scale: 1.0 - offset: -0.397411 - mech_limits: [-0.785398, 0.785398] - - # --- Joint 7 --- - - leader: arm_joint7 - follower: openarm_joint7 - sign: 1.0 - scale: 1.0 - offset: -1.571 - mech_limits: [-1.570796, 1.570796] - - # --- Joint 8 --- - - leader: arm_joint8 - follower: openarm_gripper - sign: 1.0 - scale: 1.0 - offset: 0.0 - open_range: [-0.8, 0.3] - mech_limits: [-1.5707996, 0.1] - leader_range: [0.0, -0.96] \ No newline at end of file diff --git a/src/openarm_ker/ker_stream.py b/src/openarm_ker/ker_stream.py new file mode 100644 index 0000000..f0093d0 --- /dev/null +++ b/src/openarm_ker/ker_stream.py @@ -0,0 +1,457 @@ +# Copyright 2026 Enactic, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Module for streaming data from KER devices via USB or Serial transport. + +This module provides the `KERStream` class to handle protocol handshaking, +schema fetching, and continuous asynchronous data reading. +""" + +import struct +import threading +import time +from queue import Queue, Empty +from typing import Any + +# ===================================================== +# Protocol Constants +# ===================================================== +HEADER_STREAM = b"\xa5\x5a" +HEADER_PING = b"\xa5\x50" + +CMD_PING = b"\x00" +CMD_STANDBY = b"\x01" +CMD_STREAM = b"\x02" + +TYPE_MAP = { + 0: ("I", 4, "UINT32"), + 1: ("H", 2, "UINT16"), + 2: ("B", 1, "UINT8"), + 3: ("i", 4, "INT32"), + 4: ("h", 2, "INT16"), + 5: ("f", 4, "FLOAT"), + 6: ("?", 1, "BOOL"), +} + + +def _verify_checksum(packet: bytes) -> bool: + """Verify the checksum of a given byte packet.""" + cs = 0 + for b in packet[2:-1]: + cs ^= b + return cs == packet[-1] + + +class KERStream: + """Handler for KER device communication. + + Establishes a connection (USB/Serial), retrieves the schema, + and maintains a background thread to read the latest streaming data. + """ + + def __init__( + self, + transport: str = "usb", + port: str = "/dev/ttyACM0", + baud: int = 2000000, + vid: int = 0x303A, + pid: int = 0x4002, + timeout: float = 0.01, + ): + """Initialize the stream configuration.""" + self._transport = transport + self._port = port + self._baud = baud + self._vid = vid + self._pid = pid + self._timeout = timeout + + self._dev = None + self._ep_in = None + self._ep_out = None + self._serial = None + self._buf = bytearray() + + self.metadata = {} + self._fields = [] + self._fmt = "" + self._packet_size = 0 + + # Read thread + self._latest_data = None + self._lock = threading.Lock() + self._queue = Queue(maxsize=2) + self._running = False + self._thread = None + + # -------------------------------------------------- + # Connect + # -------------------------------------------------- + def connect(self): + """Establish connection to the hardware and start the read thread.""" + if self._transport == "usb": + self._connect_usb() + elif self._transport == "serial": + self._connect_serial() + else: + raise ValueError(f"Unknown transport: {self._transport}") + + self._ping_and_fetch_schema() + + self._running = True + self._thread = threading.Thread(target=self._read_loop, daemon=True) + self._thread.start() + + def _connect_usb(self): + import usb.core + import usb.util + + dev = usb.core.find(idVendor=self._vid, idProduct=self._pid) + if dev is None: + raise RuntimeError( + f"USB device {self._vid:#06x}:{self._pid:#06x} not found" + ) + + if dev.is_kernel_driver_active(0): + dev.detach_kernel_driver(0) + + dev.set_configuration() + cfg = dev.get_active_configuration() + intf = cfg[(0, 0)] + + self._ep_in = usb.util.find_descriptor( + intf, + custom_match=lambda e: usb.util.endpoint_direction(e.bEndpointAddress) + == usb.util.ENDPOINT_IN, + ) + self._ep_out = usb.util.find_descriptor( + intf, + custom_match=lambda e: usb.util.endpoint_direction(e.bEndpointAddress) + == usb.util.ENDPOINT_OUT, + ) + self._dev = dev + + try: + self._dev.write(self._ep_out.bEndpointAddress, CMD_STANDBY) + except Exception: + pass + + flush_end = time.time() + 0.2 + while time.time() < flush_end: + try: + self._dev.read(self._ep_in.bEndpointAddress, 512, timeout=10) + except usb.core.USBError: + break + + def _connect_serial(self): + import serial + + self._serial = serial.Serial( + port=self._port, baudrate=self._baud, timeout=self._timeout + ) + + try: + self._serial.write(CMD_STANDBY) + time.sleep(0.05) + except Exception: + pass + self._serial.reset_input_buffer() + + # -------------------------------------------------- + # Connection Status Property + # -------------------------------------------------- + @property + def is_connected(self) -> bool: + """Return whether the stream is currently connected and running.""" + return self._running + + # -------------------------------------------------- + # Command: Ping Only + # -------------------------------------------------- + def ping_only(self) -> dict[str, Any] | None: + """Connect temporarily to fetch device metadata. + + Sends a PING command to fetch device metadata and fields schema, + then cleanly disconnects without starting the stream thread. + + Returns: + Dictionary containing metadata, or None if it fails. + + """ + try: + if self._transport == "usb": + self._connect_usb() + elif self._transport == "serial": + self._connect_serial() + else: + raise ValueError(f"Unknown transport: {self._transport}") + + self._ping_and_fetch_schema() + return self.metadata + + except Exception as e: + print(f"[Ping Failed] Error: {e}") + return None + + finally: + self.close() + + # -------------------------------------------------- + # Handshake & Schema parsing + # -------------------------------------------------- + def _ping_and_fetch_schema(self): + self._buf.clear() + + start_time = time.time() + last_ping = 0 + + while time.time() - start_time < 3.0: + if time.time() - last_ping >= 0.5: + try: + self.send_command(CMD_PING) + except Exception: + pass + last_ping = time.time() + + chunk = self._read_raw(512) + if chunk: + self._buf.extend(chunk) + + idx = self._buf.find(HEADER_PING) + if idx != -1: + self._buf = self._buf[idx:] + if self._parse_ping_response(): + return + + raise TimeoutError( + f"Failed to fetch schema. Received buffer: {self._buf.hex()}" + ) + + def _parse_ping_response(self) -> bool: + if len(self._buf) < 47: + return False + + pos = 2 + fw = self._buf[pos : pos + 16].decode("utf-8", "ignore").rstrip("\x00") + pos += 16 + hw = self._buf[pos : pos + 16].decode("utf-8", "ignore").rstrip("\x00") + pos += 16 + updated = self._buf[pos : pos + 12].decode("utf-8", "ignore").rstrip("\x00") + pos += 12 + + self.metadata = {"fw": fw, "hw": hw, "updated": updated} + + field_count = self._buf[pos] + pos += 1 + + if len(self._buf) < pos + (field_count * 18): + return False + + self._fields = [] + fmt_str = "<" + + for _ in range(field_count): + key = self._buf[pos : pos + 16].decode("utf-8", "ignore").rstrip("\x00") + pos += 16 + type_id = self._buf[pos] + pos += 1 + count = self._buf[pos] + pos += 1 + + fmt_char, _, _ = TYPE_MAP.get(type_id, ("x", 1, "UNKNOWN")) + self._fields.append({"key": key, "count": count, "format": fmt_char}) + fmt_str += f"{count}{fmt_char}" if count > 1 else fmt_char + + self._fmt = fmt_str + self._packet_size = 2 + struct.calcsize(self._fmt) + 1 + + self._buf.clear() + return True + + # -------------------------------------------------- + # Read thread + # -------------------------------------------------- + def _enqueue(self, d: dict[str, Any]) -> None: + if self._queue.full(): + try: + self._queue.get_nowait() + except Empty: + pass + self._queue.put_nowait(d) + + def _read_loop(self): + while self._running: + packets = self._read_all() + for d in packets: + with self._lock: + self._latest_data = d + self._enqueue(d) + if not packets: + time.sleep(0.001) + + def latest(self) -> dict[str, Any] | None: + """Retrieve the most recently parsed data packet. + + Returns: + A dictionary of parsed fields, or None if no data is available yet. + + """ + with self._lock: + return self._latest_data + + def recv(self) -> dict[str, Any] | None: + """Get next packet from queue. + + Returns: + A dictionary of parsed fields, or None if queue is empty. + + """ + try: + return self._queue.get_nowait() + except Empty: + return None + + # -------------------------------------------------- + # Internal read + # -------------------------------------------------- + def _read_all(self) -> list[dict[str, Any]]: + chunk = self._read_raw(4096) + if chunk: + self._buf.extend(chunk) + + results = [] + + while len(self._buf) >= self._packet_size: + idx = self._buf.find(HEADER_STREAM) + if idx == -1: + self._buf.clear() + break + if idx > 0: + self._buf = self._buf[idx:] + if len(self._buf) < self._packet_size: + break + + packet = bytes(self._buf[: self._packet_size]) + self._buf = self._buf[self._packet_size :] + + if not _verify_checksum(packet): + continue + + results.append(self._parse_stream_packet(packet)) + + return results + + def _read_raw(self, size) -> bytes: + if self._transport == "usb": + import usb.core + + if self._dev is None: + return b"" + + try: + return bytes( + self._dev.read(self._ep_in.bEndpointAddress, size, timeout=20) + ) + except usb.core.USBError as e: + if e.errno in (110, 116) or "timeout" in str(e).lower(): + return b"" + print("\n[Disconnected] USB disconnected... now lose connect ,,,,") + self._running = False + return b"" + except Exception as e: + print(f"\n[Error] Unexpected error: {e}") + self._running = False + return b"" + else: + import serial + + if self._serial is None: + return b"" + + try: + waiting = self._serial.in_waiting + if waiting > 0: + return self._serial.read(min(waiting, size)) + else: + return b"" + except serial.SerialException: + print("\n[Disconnected] Serial disconnected... now lose connect ,,,,") + self._running = False + return b"" + except Exception as e: + print(f"\n[Error] Unexpected error: {e}") + self._running = False + return b"" + + def _parse_stream_packet(self, packet: bytes) -> dict[str, Any]: + unpacked = struct.unpack(self._fmt, packet[2:-1]) + + data = {} + index = 0 + for f in self._fields: + key = f["key"] + count = f["count"] + if count == 1: + data[key] = unpacked[index] + else: + data[key] = list(unpacked[index : index + count]) + index += count + + return data + + # -------------------------------------------------- + # Send / Close + # -------------------------------------------------- + def send_command(self, cmd: bytes): + """Send a raw byte command to the connected device.""" + if self._transport == "usb": + if self._ep_out is None: + raise RuntimeError("USB not connected") + self._dev.write(self._ep_out.bEndpointAddress, cmd) + else: + if self._serial is None: + raise RuntimeError("Serial not connected") + self._serial.write(cmd) + + def close(self): + """Terminate the stream thread and release hardware resources.""" + self._running = False + if self._thread is not None: + self._thread.join(timeout=1.0) + self._thread = None + + if self._serial is not None: + self._serial.close() + self._serial = None + + if self._transport == "usb" and self._dev is not None: + import usb.util + + try: + usb.util.dispose_resources(self._dev) + except Exception: + pass + + self._dev = None + self._ep_in = None + self._ep_out = None + + def __enter__(self): + """Enter context manager, automatically connecting to the device.""" + self.connect() + return self + + def __exit__(self, *args): + """Exit context manager, automatically closing the connection.""" + self.close() diff --git a/src/openarm_ker/m5_port.py b/src/openarm_ker/m5_port.py deleted file mode 100644 index b3fe1bf..0000000 --- a/src/openarm_ker/m5_port.py +++ /dev/null @@ -1,292 +0,0 @@ -# Copyright 2026 Enactic, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""M5 Encoder Port for reading MT6835 angles via Serial.""" - -import serial -import struct -import numpy as np -import time -import json - - -class M5Port: - """M5 Encoder Port for reading MT6835 angles via Serial. - - Supports both 'binary' (production/fast) and 'json' (debugging) modes. - """ - - def __init__( - self, - device, - num_sensors=16, - baudrate=2000000, - timeout=0.005, - mode="json", - ): - """Initialize M5 port. - - Args: - device: Serial port device (e.g., '/dev/ttyACM0', 'COM3'). - num_sensors: Number of encoder sensors (default: 16) - baudrate: Serial baud rate (default: 2000000). - timeout: Serial timeout in seconds. (0.05 ensures OS-level blocking to drop CPU usage) - mode: 'binary' or 'json' - - """ - self.device = device - self.num_sensors = num_sensors - self.baudrate = baudrate - self.mode = mode.lower() - - # Initialize arrays (store in degrees) - self.present_position = np.full(num_sensors, np.nan) - self.present_errors = np.zeros(num_sensors, dtype=bool) - self.timestamp = 0 - - # Initialize peripherals - self.chain_encoder = 0 - self.chain_encoder_button = 0 - self.joystick_x = 0 - self.joystick_y = 0 - self.joystick_button = 0 - - # Buffers for serial data - self.byte_buffer = bytearray() - self.string_buffer = "" - - # Binary packet format: - # < : Little Endian - # H : Header (2 bytes) - # I : Timestamp (4 bytes) - # 16f : Angles (16 * 4 = 64 bytes) - # H : Error Mask (2 bytes) - # h : Chain Encoder (2 bytes) - # h : Joystick X (2 bytes) - # h : Joystick Y (2 bytes) - # B : Buttons Mask (1 byte) - # B : Checksum (1 byte) - # Total: 80 bytes - self.PACKET_FORMAT = " 4096: - del self.byte_buffer[: -self.PACKET_SIZE * 2] - - parsed_any = False - - # Process all complete packets in the buffer - while len(self.byte_buffer) >= self.PACKET_SIZE: - # Search for Little-Endian header: 0xA55A (0x5A, 0xA5) - if self.byte_buffer[0] == 0x5A and self.byte_buffer[1] == 0xA5: - packet = self.byte_buffer[: self.PACKET_SIZE] - - # Calculate and verify XOR checksum - calculated_checksum = 0 - for i in range(self.PACKET_SIZE - 1): - calculated_checksum ^= packet[i] - - if calculated_checksum == packet[-1]: - # Valid packet! Parse it. - self._parse_binary_packet(packet) - parsed_any = True - del self.byte_buffer[: self.PACKET_SIZE] - else: - # Corrupted data. Drop the first byte to re-align. - del self.byte_buffer[0:1] - else: - # Header not found at start. Drop 1 byte and search again. - del self.byte_buffer[0:1] - - return parsed_any - - except Exception: - return False - - def _parse_binary_packet(self, packet): - """Unpack strictly mapped C++ struct.""" - data = struct.unpack(self.PACKET_FORMAT, packet) - - self.timestamp = data[1] - angles = data[2:18] - error_mask = data[18] - - for i in range(self.num_sensors): - is_err = bool((error_mask >> i) & 1) - self.present_errors[i] = is_err - if is_err: - self.present_position[i] = np.nan - else: - self.present_position[i] = angles[i] - - self.chain_encoder = data[19] - self.joystick_x = data[20] - self.joystick_y = data[21] - - button_mask = data[22] - self.chain_encoder_button = button_mask & 0x01 - self.joystick_button = (button_mask >> 1) & 0x01 - - # --------------------------------------------------------- - # JSON PARSING LOGIC - # --------------------------------------------------------- - def _read_json(self): - try: - to_read = max(1, self.serial.in_waiting) - chunk = self.serial.read(to_read).decode("utf-8", errors="ignore") - - if not chunk: - return False - - self.string_buffer += chunk - - latest_valid_line = None - while "\n" in self.string_buffer: - line, self.string_buffer = self.string_buffer.split("\n", 1) - line = line.strip() - if line: - latest_valid_line = line - - if latest_valid_line: - data = json.loads(latest_valid_line) - - self.timestamp = data.get("timestamp", data.get("ts", 0)) - angles = data.get("angles", data.get("ang", [])) - errors = data.get("errors", data.get("err", [])) - - for i in range(min(len(angles), self.num_sensors)): - angle = angles[i] - is_error = (angle is None) or (i < len(errors) and errors[i]) - self.present_errors[i] = is_error - self.present_position[i] = np.nan if is_error else float(angle) - - self.chain_encoder = data.get("u_enc", 0) - self.chain_encoder_button = data.get("u_btn", 0) - self.joystick_x = data.get("jx", 0) - self.joystick_y = data.get("jy", 0) - self.joystick_button = data.get("jbtn", 0) - - return True - except Exception: - # Silently ignore JSON parse errors which are common on startup - pass - return False - - # --------------------------------------------------------- - # PUBLIC API / GETTERS - # --------------------------------------------------------- - def fetch_present_status(self): - """Fetch present status.""" - self._read_once() - - def fetch_present_status_bulk(self): - """Fetch present status in bulk.""" - self._read_once() - - def cleanup(self): - """Close connections.""" - try: - if hasattr(self, "serial") and self.serial.is_open: - self.serial.close() - except Exception: - pass - - def get_angles_degrees(self): - """Get angels in degrees.""" - return self.present_position.copy() - - def get_angles_radians(self): - """Get angels in radians.""" - return self.present_position * (np.pi / 180.0) - - def get_chain_encoder(self): - """Get chain encoder.""" - return self.chain_encoder - - def get_chain_button(self): - """Get chain button.""" - return self.chain_encoder_button - - def get_joystick_x_raw(self): - """Get the raw value of joystick x.""" - return self.joystick_x - - def get_joystick_y_raw(self): - """Get the raw value of joystick y.""" - return self.joystick_y - - def get_joystick_x(self): - """Get the mapped joystick x value (-1.0 to 1.0).""" - if self.joystick_x > (1 << 12) - 1: - return (self.joystick_x - ((1 << 16) - 1)) / ((1 << 12) - 1) - return self.joystick_x / ((1 << 12) - 1) - - def get_joystick_y(self): - """Get the mapped joystick y value (-1.0 to 1.0).""" - if self.joystick_y > (1 << 12) - 1: - return (self.joystick_y - ((1 << 16) - 1)) / ((1 << 12) - 1) - return self.joystick_y / ((1 << 12) - 1) - - def get_joystick_button(self): - """Get joystick button.""" - return self.joystick_button - - def has_errors(self): - """Return whether an error has occurred.""" - return np.any(self.present_errors) - - def get_error_status(self): - """Get errors.""" - return self.present_errors.copy() diff --git a/src/openarm_ker/mapper.py b/src/openarm_ker/mapper.py deleted file mode 100644 index 5e65e2f..0000000 --- a/src/openarm_ker/mapper.py +++ /dev/null @@ -1,141 +0,0 @@ -# Copyright 2026 Enactic, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Joint mapper.""" - -from __future__ import annotations - -import importlib.resources -from importlib.resources.abc import Traversable -from pathlib import Path - -import yaml -import numpy as np - - -class Mapper: - """Joint mapper.""" - - def __init__( - self, - mappingyaml_path: str | Path, - leader_joint_names: list[str], - mapping_key: str, - ): - """Initialize joint mapper.""" - # Load YAML configuration - mappingyaml_file = self._resolve_path(mappingyaml_path) - with mappingyaml_file.open() as f: - full_config = yaml.safe_load(f) - - if mapping_key not in full_config: - raise ValueError(f"Section '{mapping_key}' not found in YAML.") - - mapping_list = full_config[mapping_key] - self.num_joints = len(mapping_list) - - # Allocate NumPy arrays - self.indices = np.zeros(self.num_joints, dtype=int) - self.scales = np.ones(self.num_joints, dtype=float) - self.offsets = np.zeros(self.num_joints, dtype=float) - - # Allocate Limit arrays - self.limits_min = np.full(self.num_joints, -100.0, dtype=float) - self.limits_max = np.full(self.num_joints, 100.0, dtype=float) - - leader_name_to_idx = {name: i for i, name in enumerate(leader_joint_names)} - self.follower_names = [] - - # --- Process Mappings --- - for i, m in enumerate(mapping_list): - l_name = m["leader"] - f_name = m["follower"] - - if l_name not in leader_name_to_idx: - raise ValueError(f"Leader joint '{l_name}' not found.") - - self.indices[i] = leader_name_to_idx[l_name] - self.scales[i] = m.get("sign", 1.0) * m.get("scale", 1.0) - self.offsets[i] = m.get("offset", 0.0) - self.follower_names.append(f_name) - - self.open_range = m.get("open_range") - self.leader_range = m.get("leader_range") - - mech_limit = m.get("mech_limits") - - # 0.25 radian margin. If target exceeds this, follower will be inverted - if mech_limit and isinstance(mech_limit, list) and len(mech_limit) == 2: - self.limits_min[i] = mech_limit[0] - 0.35 - self.limits_max[i] = mech_limit[1] + 0.35 - else: - raise ValueError(f"Invalid mech_limits for joint '{f_name}'.") - - self.TWO_PI = 2 * np.pi - - @staticmethod - def _resolve_path(mappingyaml_path: str | Path) -> Path | Traversable: - """Resolve a user path or bundled config filename.""" - path = Path(mappingyaml_path) - if path.exists(): - if path.is_file(): - return path - raise FileNotFoundError(f"Mapping YAML path is not a file: {path}") - - if len(path.parts) == 1: - resource = importlib.resources.files("openarm_ker").joinpath( - "config", path.name - ) - if resource.is_file(): - return resource - - raise FileNotFoundError( - f"Mapping YAML file not found: {mappingyaml_path}. " - "Pass an existing file path or a bundled config filename under " - "openarm_ker/config/." - ) - - def __map_range(self, in_min, in_max, out_min, out_max, val): - """Map value from one range to another.""" - return (val - in_min) / (in_max - in_min) * (out_max - out_min) + out_min - - def map(self, leader_position: np.ndarray) -> np.ndarray: - """Execute mapping. - - Logic: - 1. Apply linear transform FIRST (y = x * scale - offset). - 2. Then check if the RESULT is within limits. - 3. Wrap output values (add/sub 2pi) if they are outside limits. - """ - # 1. Linear Transformation (Calculate Follower Command directly) - # target_vals = leader_position[self.indices] * self.scales - self.offsets - target_vals = (leader_position[self.indices] - self.offsets) * self.scales - - follower_gripper_pos = self.__map_range( - in_max=self.leader_range[1], - in_min=self.leader_range[0], - out_max=self.open_range[1], - out_min=self.open_range[0], - val=target_vals[-1], - ) - target_vals[-1] = follower_gripper_pos - - # 2. Output Wrapping (Check result against limits) to joint1 ~ joint7 - for i in range(self.num_joints - 1): - if target_vals[i] < self.limits_min[i]: - target_vals[i] += self.TWO_PI - elif target_vals[i] > self.limits_max[i]: - target_vals[i] -= self.TWO_PI - - return target_vals