diff --git a/FAQ_en.md b/FAQ_en.md index b484cbb..509db81 100644 --- a/FAQ_en.md +++ b/FAQ_en.md @@ -151,4 +151,49 @@ press `q` to exit. unitree@ubuntu:~$ ping 8.8.8.8 unitree@ubuntu:~$ ping marketplace.visualstudio.com ``` -If successful, VS Code Remote SSH should now work properly. \ No newline at end of file +If successful, VS Code Remote SSH should now work properly. + + +## 7. Revo2Touch hand: `get_touch_sensor_status: deprecated for current firmware` + +The `libbc_stark_sdk.so` currently shipped via `download-lib.sh` is version +**0.4.3**, which hard-codes every touch-sensor API as "deprecated for current +firmware" and returns empty regardless of the actual hardware: + +``` +WARN touch_sensor_setup: deprecated for current firmware +WARN touch_sensor_calibrate: deprecated for current firmware +WARN get_touch_sensor_status: deprecated for current firmware, return empty +WARN stark_get_touch_sensor_raw_data: deprecated for current firmware, return empty +``` + +This happens even on **Revo2Touch** hardware where +`modbus_get_device_info` reports `hardware_type = 4` +(`STARK_HARDWARE_TYPE_REVO2_TOUCH`) and the physical touch sensors are +installed and alive. + +There are two fixes: + +**Option A — upgrade the SDK (requires re-download).** +Version **0.8.1** of the SDK (shipped with +[`unitreerobotics/brainco_hand_service`](https://github.com/unitreerobotics/brainco_hand_service)) +adds `STARK_HARDWARE_TYPE_REVO2_TOUCH = 4` and accepts the touch-API +calls. Drop that `libbc_stark_sdk.so` into +`ros2_stark_ws/src/ros2_stark_controller/lib/`, update `stark-sdk.h` to +the matching version, and set `firmware_type: 4` in +`config/params_v2_double.yaml`. + +**Option B — skip the SDK and talk raw Modbus.** +A standalone workaround script that uses only `pyserial` and talks the +same Modbus RTU commands the newer SDK sends internally: + +```sh +pip install pyserial +python3 ros2_stark_ws/src/ros2_stark_controller/scripts/touch_sensor_pyserial.py --dual +``` + +It enables the touch sensors, calibrates the idle baseline, and +polls register `4200` at 10 Hz. Pressing a fingertip shows the per-finger +normal_force going from 0 to ~2500. Works on any installed SDK version +because it bypasses `libbc_stark_sdk.so` entirely. See the top of that +file for the register-layout documentation. \ No newline at end of file diff --git a/ros2_stark_ws/src/ros2_stark_controller/scripts/touch_sensor_pyserial.py b/ros2_stark_ws/src/ros2_stark_controller/scripts/touch_sensor_pyserial.py new file mode 100755 index 0000000..81d0b8a --- /dev/null +++ b/ros2_stark_ws/src/ros2_stark_controller/scripts/touch_sensor_pyserial.py @@ -0,0 +1,240 @@ +#!/usr/bin/env python3 +""" +Standalone pyserial workaround for reading Brainco Revo2Touch force sensors +on installations where the bundled libbc_stark_sdk.so version is too old to +expose the touch API. + +Problem +------- +The `libbc_stark_sdk.so` shipped in this repo (version 0.4.3) hard-codes the +touch-sensor APIs as "deprecated for current firmware, return empty": + + WARN touch_sensor_setup: deprecated for current firmware + WARN touch_sensor_calibrate: deprecated for current firmware + WARN get_touch_sensor_status: deprecated for current firmware, return empty + +even on hardware where `modbus_get_device_info` returns +`hardware_type = 4` (STARK_HARDWARE_TYPE_REVO2_TOUCH). Version 0.8.1 of +the SDK (shipped with the sibling repo `unitreerobotics/brainco_hand_service`) +does expose those APIs and successfully talks to the same hands — the +fix is purely on the SDK side, not the firmware. + +This script sidesteps the SDK version mismatch entirely by speaking the +same Modbus RTU commands the newer SDK would send, using only `pyserial`. +It works on any Revo2Touch hand regardless of which version of +`libbc_stark_sdk.so` is installed. + +Usage +----- + + pip install pyserial + sudo chmod 666 /dev/ttyUSB1 /dev/ttyUSB2 # or add user to dialout + python3 touch_sensor_pyserial.py --port /dev/ttyUSB1 --slave 0x7e + + # or point it at both hands at once + python3 touch_sensor_pyserial.py --dual + +Press a fingertip and watch the per-finger `normal_force` reading go from +~0 to ~2500. + +Modbus registers (verified on Revo2Touch firmware 1.0.14.U) +----------------------------------------------------------- + + * Enable : WRITE holding registers @4000, data = [1, 1, 1, 1, 1] + * Calibrate : WRITE holding registers @4010, data = [1, 1, 1, 1, 1] + * Status : READ input registers @4200, count = 30 + +Register-4200 layout (30 × uint16): + + regs[0..14] = 3 per finger, interleaved f0..f4 (normal, tangential, tang_dir) + regs[15..24] = 2 per finger, interleaved f0..f4 (self_proximity lo/hi u32 halves) + regs[25..29] = per-finger frame counter / status — ignore + +Finger order: `f0=thumb, f1=index, f2=middle, f3=ring, f4=pinky`. + +The firmware writes 0xFFFF as a "no data" sentinel on idle tangential +fields; this reader masks those to 0 so callers never see a bogus +~65535 as if it were a real force. +""" + +import argparse +import struct +import sys +import time + +try: + import serial +except ImportError: + print("ERROR: pyserial is required. Install with `pip install pyserial`.") + sys.exit(1) + +# ----------------------------------------------------------------------------- +# Modbus RTU helpers (function 0x04 read input regs, 0x10 write multiple regs) +# ----------------------------------------------------------------------------- + + +def _crc16(data: bytes) -> int: + crc = 0xFFFF + for b in data: + crc ^= b + for _ in range(8): + if crc & 1: + crc = (crc >> 1) ^ 0xA001 + else: + crc >>= 1 + return crc + + +def read_input_registers(ser, slave_id, addr, count): + """Function 0x04. Returns a list of `count` uint16 or None on failure.""" + msg = struct.pack(">BBHH", slave_id, 0x04, addr, count) + msg += struct.pack("" + "H" * count, resp[3:3 + count * 2])) + + +def write_multiple_registers(ser, slave_id, addr, values): + """Function 0x10. Returns True on success.""" + count = len(values) + msg = struct.pack(">BBHHB", slave_id, 0x10, addr, count, count * 2) + for v in values: + msg += struct.pack(">H", int(v) & 0xFFFF) + msg += struct.pack(" Opening {port} slave=0x{slave_id:02x} ({label})", flush=True) + ser = serial.Serial(port, baud, timeout=0.3) + if not enable_touch(ser, slave_id): + print(f" WARN: touch enable write failed on {label}", flush=True) + else: + print(f" touch enable + calibrate: ok", flush=True) + return ser + + +def _print_row(label, parsed): + normal, tangential, _tdir, _plo, _phi = parsed + cells = [] + for i, name in enumerate(FINGER_NAMES): + marker = "*" if normal[i] > 500 else " " + cells.append(f"{marker}{name[0]}={normal[i]:4d}") + print(f" {label}: " + " ".join(cells), flush=True) + + +def main(): + p = argparse.ArgumentParser(description=__doc__) + p.add_argument("--port", default="/dev/ttyUSB1", help="serial port (default /dev/ttyUSB1)") + p.add_argument("--slave", type=lambda x: int(x, 0), default=0x7e, + help="Modbus slave id (left=0x7e, right=0x7f; default 0x7e)") + p.add_argument("--baud", type=int, default=460800, + help="serial baudrate (default 460800)") + p.add_argument("--dual", action="store_true", + help="poll both hands (/dev/ttyUSB1 + /dev/ttyUSB2)") + p.add_argument("--seconds", type=float, default=30.0, + help="how long to poll (default 30)") + p.add_argument("--rate", type=float, default=10.0, + help="poll rate in Hz (default 10)") + args = p.parse_args() + + if args.dual: + left = _open_and_enable("/dev/ttyUSB1", args.baud, 0x7e, "LEFT") + right = _open_and_enable("/dev/ttyUSB2", args.baud, 0x7f, "RIGHT") + handles = [("LEFT ", left, 0x7e), ("RIGHT", right, 0x7f)] + else: + ser = _open_and_enable(args.port, args.baud, args.slave, "HAND") + handles = [("HAND ", ser, args.slave)] + + print("", flush=True) + print(f"Polling for {args.seconds:.0f} seconds — press fingertips to see values.", + flush=True) + print("Format: finger=normal_force; '*' marks >500 (firm contact).", flush=True) + print("", flush=True) + + t_end = time.time() + args.seconds + interval = 1.0 / args.rate + while time.time() < t_end: + for label, ser, sid in handles: + parsed = read_touch(ser, sid) + if parsed is not None: + _print_row(label, parsed) + else: + print(f" {label}: (read failed)", flush=True) + time.sleep(interval) + + for _, ser, _ in handles: + ser.close() + + +if __name__ == "__main__": + main()