diff --git a/examples/o30_classic_can.py b/examples/o30_classic_can.py new file mode 100644 index 0000000..b0b54ef --- /dev/null +++ b/examples/o30_classic_can.py @@ -0,0 +1,404 @@ +"""O30 HOP over SocketCAN classic CAN: read, poll, optionally write and move. + +Run: python examples/o30_classic_can.py --channel can0 --side right +The adapter and hand must already use classic CAN at the same nominal bitrate. +This script does not change CAN mode, bitrate, CAN ID or flash settings. +""" + +from __future__ import annotations + +import argparse +import json +import math +import time +from dataclasses import asdict, is_dataclass +from enum import Enum +from pathlib import Path + +import can + +from linkerbot_hand import ( + CommandStage, + DeviceConfig, + HandDevice, + PollingPlan, + TransportKind, + UnsupportedFirmwareError, +) +from linkerbot_hand.protocols.hop.framing import build_read_message +from linkerbot_hand.protocols.hop.response import HOPRangeAssembler +from linkerbot_hand.protocols.o30.codec import decode_v25 +from linkerbot_transport.can import CANTransport + +READS = ( + "angles", + "speeds", + "configured_speeds", + "configured_accelerations", + "currents", + "current_limits", + "torques", + "torques_limits", + "temperatures", + "voltages", + "motion_times", + "stall_detection_times", + "stall_thresholds", + "stall_hold_currents", + "faults", +) +WRITE_BACK = ( + ("configured_speeds", "set_speeds"), + ("configured_accelerations", "set_accelerations"), + ("current_limits", "set_current_limits"), + ("torques_limits", "set_torques_limits"), + ("motion_times", "set_motion_times"), + ("stall_detection_times", "set_stall_detection_times"), + ("stall_thresholds", "set_stall_thresholds"), + ("stall_hold_currents", "set_stall_hold_currents"), +) + + +def encode(value): + if isinstance(value, Enum): + return value.value + if is_dataclass(value) and not isinstance(value, type): + return {k: encode(v) for k, v in asdict(value).items()} + if isinstance(value, dict): + return {str(k): encode(v) for k, v in value.items()} + if isinstance(value, (tuple, list)): + return [encode(v) for v in value] + if isinstance(value, bytes): + return value.hex() + return value + + +def checked(command): + stage = command.wait(timeout=3) + if stage is not CommandStage.ACKNOWLEDGED: + raise RuntimeError(f"command {stage.value}: {command.error}") + return {"stage": stage.value} + + +def read_targets(channel, request_id, timeout=1.0): + """Read O30 RTS=1 targets for this script, while SDK polling is stopped. + + This isolated validation helper reuses the SDK codec. Do not run another + query or command producer on the same endpoint during the exercise. + """ + assembler = HOPRangeAssembler( + 1, sub_index=0, length=25, rts=True, response_id=request_id | 0x400 + ) + with can.Bus(interface="socketcan", channel=channel) as bus: + bus.set_filters( + [{"can_id": request_id | 0x400, "can_mask": 0x7FF, "extended": False}] + ) + deadline = time.monotonic() + timeout + for start in range(0, 25, 5): + remaining = deadline - time.monotonic() + if remaining <= 0: + break + bus.send( + build_read_message( + 1, + sub_index=start, + length=5, + request_id=request_id, + rts=True, + can_fd=False, + ), + timeout=remaining, + ) + current = HOPRangeAssembler( + 1, sub_index=start, length=5, rts=True, response_id=request_id | 0x400 + ) + chunk = None + while (remaining := deadline - time.monotonic()) > 0: + message = bus.recv(remaining) + if message is None: + break + matched, chunk = current.consume(message) + if matched: + _, complete = assembler.consume(message) + if complete is not None: + return decode_v25(complete) + if chunk is not None: + break + if chunk is None: + break + raise TimeoutError("O30 target register read did not complete") + + +def observe_angles(hand, duration, trace): + """Observe fresh feedback for a time window without assuming arrival.""" + deadline = time.monotonic() + duration + latest = None + while latest is None or time.monotonic() < deadline: + # A request gets its own response budget even near the window boundary. + latest = hand.get_angles(timeout=1.0).raw + trace.append(latest) + time.sleep(min(0.01, max(0, deadline - time.monotonic()))) + return latest + + +def exercise_motion(hand, args, report, target_reader): + joint = args.motion_joint + original_targets = tuple(target_reader()) + initial = hand.get_angles(timeout=3).raw + target = list(original_targets) + target[joint] += args.delta + motion = report["motion"] = { + "original_targets": original_targets, + "initial_actual": initial, + "target": target, + "joint": joint, + "observation_duration_s": args.motion_timeout, + "observations": [], + "restore_observations": [], + } + if not 0 <= target[joint] <= 255: + raise ValueError("motion target is outside 0..255; choose another delta") + try: + motion["command"] = checked(hand.set_angles(target)) + motion["target_readback"] = tuple(target_reader()) + if motion["target_readback"] != tuple(target): + raise RuntimeError("motion target register readback mismatch") + motion["observed"] = observe_angles( + hand, args.motion_timeout, motion["observations"] + ) + except Exception as error: + motion["command_error"] = f"{type(error).__name__}: {error}" + raise + finally: + # Keep restoration errors in the report even when an earlier step fails. + try: + motion["restore"] = checked(hand.set_angles(original_targets)) + motion["restored_targets"] = tuple(target_reader()) + if motion["restored_targets"] != original_targets: + raise RuntimeError("original target register restoration failed") + motion["restored_observed"] = observe_angles( + hand, args.motion_timeout, motion["restore_observations"] + ) + except Exception as error: + motion["restore_error"] = f"{type(error).__name__}: {error}" + raise + motion["signed_motion_raw"] = (motion["observed"][joint] - initial[joint]) * ( + 1 if args.delta > 0 else -1 + ) + motion["target_error_raw"] = abs(motion["observed"][joint] - target[joint]) + motion["restore_error_raw"] = abs( + motion["restored_observed"][joint] - initial[joint] + ) + motion["restored_target_error_raw"] = abs( + motion["restored_observed"][joint] - original_targets[joint] + ) + motion["actual_motion_observed"] = any( + sample[joint] != initial[joint] for sample in motion["observations"] + ) + motion["other_joint_max_change_raw"] = max( + abs(value - before) + for sample in motion["observations"] + motion["restore_observations"] + for index, (value, before) in enumerate(zip(sample, initial, strict=True)) + if index != joint + ) + motion["status"] = "TARGETS_VERIFIED" + print("motion: targets verified; actual feedback is recorded separately") + + +def exercise(hand, args, report, *, target_reader=None): + """Exercise an opened device; ordinary read/write errors fail the run.""" + report["identity"] = encode(hand.get_device_info(timeout=3)) + report["reads"] = {} + for name in READS: + sample = getattr(hand, "get_" + name)(timeout=3) + report["reads"][name] = encode(sample) + print(f"{name}: {sample.raw}") + try: + report["force"] = encode(hand.get_force("fn", timeout=5)) + print("force: received (see report for supported fields and validity)") + except UnsupportedFirmwareError as error: + report["force"] = {"status": "SKIP", "reason": str(error)} + print(f"force: SKIP ({error})") + + if args.write_back: + report["write_back"] = {} + for name, setter in WRITE_BACK: + before = report["reads"][name]["raw"] + entry = report["write_back"][name] = checked(getattr(hand, setter)(before)) + after = getattr(hand, "get_" + name)(timeout=3).raw + entry["readback"] = encode(after) + if list(after) != before: + raise RuntimeError( + f"{name}: readback differs from original configuration" + ) + print("write-back: configuration readback matched") + + if args.motion_joint is not None: + exercise_motion( + hand, + args, + report, + target_reader or (lambda: read_targets(args.channel, args.request_id)), + ) + + start_sequence = hand.get_snapshot().angles.meta.sequence + sequences = set() + hand.start_polling(PollingPlan(angles_hz=args.angles_hz, faults_hz=1)) + start = time.monotonic() + try: + while time.monotonic() - start < args.duration: + sample = hand.get_snapshot().angles + if sample.meta.valid and sample.meta.sequence > start_sequence: + sequences.add(sample.meta.sequence) + time.sleep(0.002) + finally: + hand.stop_polling() + report["polling"] = { + "requested_angles_hz": args.angles_hz, + "duration_s": time.monotonic() - start, + "observed_unique_angle_samples": len(sequences), + "latest": encode(hand.get_snapshot().angles), + } + if not sequences: + raise RuntimeError("polling produced no new angle sample") + report["health"] = encode(hand.get_health()) + print("polling:", len(sequences), "distinct angle samples observed") + health = report["health"] + recent = health["recent_error"] + expected_sensor_skip = ( + report["force"].get("status") == "SKIP" + and recent is not None + and recent["code"] == "unsupported_firmware" + and recent["operation"] == "get_force" + ) + if ( + not health["connected"] + or health["consecutive_errors"] + or (recent is not None and not expected_sensor_skip) + ): + raise RuntimeError("polling reported a communication error; see report health") + + +def positive(value): + number = float(value) + if not math.isfinite(number) or number <= 0: + raise argparse.ArgumentTypeError("must be finite and positive") + return number + + +def main(): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--channel", default="can0") + parser.add_argument("--side", choices=("left", "right"), required=True) + parser.add_argument( + "--request-id", + type=lambda v: int(v, 0), + help="actual request ID; defaults to right=1, left=2", + ) + parser.add_argument("--duration", type=positive, default=3) + parser.add_argument("--angles-hz", type=positive, default=20) + parser.add_argument( + "--write-back", + action="store_true", + help="write existing values and verify configuration readback", + ) + parser.add_argument( + "--motion-joint", + type=int, + choices=range(20), + help="change one joint target (0-based index), then restore original targets", + ) + parser.add_argument( + "--delta", type=int, default=2, help="raw angle change, -5..5 excluding zero" + ) + parser.add_argument( + "--motion-timeout", + type=positive, + default=2, + help="seconds of feedback observations after each target write", + ) + parser.add_argument( + "--report", type=Path, default=Path("o30_classic_can_report.json") + ) + args = parser.parse_args() + if args.delta == 0 or not -5 <= args.delta <= 5: + parser.error("--delta must be -5..5 excluding zero") + if args.request_id is None: + args.request_id = 1 if args.side == "right" else 2 + if not 0 <= args.request_id <= 0x3FE: + parser.error("--request-id must be 0..0x3FE") + frames = [] + report = {"arguments": vars(args) | {"report": str(args.report)}, "frames": frames} + hand = None + exit_code = 1 + + def record(message): + frames.append( + { + "timestamp": message.timestamp, + "id": message.arbitration_id, + "data": bytes(message.data).hex(), + "is_fd": message.is_fd, + "brs": message.bitrate_switch, + "error": message.is_error_frame, + "extended": message.is_extended_id, + } + ) + + try: + with can.Bus(interface="socketcan", channel=args.channel) as observer: + notifier = can.Notifier(observer, [record], timeout=0.1) + try: + hand = HandDevice( + model="O30", + side=args.side, + transport=CANTransport(args.channel), + config=DeviceConfig( + hop_request_id=args.request_id, hop_transport=TransportKind.CAN + ), + ) + hand.open() + exercise(hand, args, report) + hand.close() + hand.open() + report["reopen_angles"] = hand.get_angles(timeout=3).raw + hand.recover(timeout=5) + report["recovered_angles"] = hand.get_angles(timeout=3).raw + endpoint_frames = [ + f + for f in frames + if f["id"] in (args.request_id, args.request_id | 0x400) + ] + if not endpoint_frames or any( + f["is_fd"] + or f["brs"] + or f["error"] + or len(bytes.fromhex(f["data"])) > 8 + for f in endpoint_frames + ): + raise RuntimeError( + "expected classic CAN endpoint traffic without errors" + ) + report["status"] = "PASS" + exit_code = 0 + finally: + try: + if hand is not None: + hand.close() + finally: + notifier.stop() + except Exception as error: # noqa: BLE001 - CLI saves failures with captured frames + report["status"] = "FAIL" + report["error"] = f"{type(error).__name__}: {error}" + print(report["error"]) + finally: + args.report.parent.mkdir(parents=True, exist_ok=True) + args.report.write_text( + json.dumps(encode(report), ensure_ascii=False, indent=2) + "\n" + ) + print("report:", args.report) + return exit_code + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/examples/o6_can_mode.py b/examples/o6_can_mode.py new file mode 100644 index 0000000..badd74b --- /dev/null +++ b/examples/o6_can_mode.py @@ -0,0 +1,207 @@ +"""Standalone O6 HOP CAN mode switch; no motion and no default Flash writes.""" + +from __future__ import annotations + +import argparse +import math +import subprocess +from time import monotonic + +import can + + +def exchange(bus, request_id, data, *, can_fd, timeout=0.5, response_fd=None): + bus.send( + can.Message( + arbitration_id=request_id, + data=data, + is_extended_id=False, + is_fd=can_fd, + check=True, + ), + timeout=timeout, + ) + deadline = monotonic() + timeout + while (remaining := deadline - monotonic()) > 0: + reply = bus.recv(timeout=remaining) + if reply is None: + break + if ( + reply.is_error_frame + or reply.is_remote_frame + or reply.is_extended_id + or reply.arbitration_id != request_id | 0x400 + or (response_fd is not None and reply.is_fd != response_fd) + ): + continue + payload = bytes(reply.data) + if len(payload) < 3: + continue + control, _start, size = payload[:3] + if control & 0x7F == 0x4F: + raise RuntimeError(f"Device communication error: {payload.hex()}") + if payload[:3] != data[:3]: + continue + if len(payload) < 3 + size: + raise RuntimeError("Truncated HOP response") + return payload[3 : 3 + size], reply.is_fd + raise TimeoutError(f"No matching reply for {data.hex()}") + + +def inspect(bus, request_id, can_fd, side, timeout): + product, wire = exchange( + bus, request_id, bytes.fromhex("410005"), can_fd=can_fd, timeout=timeout + ) + model = product.split(b"\x00", 1)[0].decode("ascii") + if model.upper() not in {"O6", "LHO6"}: + raise RuntimeError(f"Expected O6, received {model!r}; device not changed") + reported_side, _ = exchange( + bus, + request_id, + bytes.fromhex("41F105"), + can_fd=can_fd, + timeout=timeout, + response_fd=wire, + ) + if reported_side.rstrip(b"\x00").decode("ascii").lower() != side: + raise RuntimeError("Device side mismatch; device not changed") + configuration, _ = exchange( + bus, + request_id, + bytes.fromhex("B50604"), + can_fd=can_fd, + timeout=timeout, + response_fd=wire, + ) + mode, brs, nominal, data = configuration + if ( + mode not in (1, 2) + or brs not in (0, 1) + or not 1 <= nominal <= 8 + or not 1 <= data <= 13 + ): + raise RuntimeError(f"Invalid device configuration: {configuration.hex()}") + print( + f"O6 {side}: reply={'canfd' if wire else 'can'}, mode={mode}, BRS={brs}", + flush=True, + ) + return configuration, wire + + +def configure_host(channel, fd, bitrate, dbitrate): + subprocess.run( + ["sudo", "-n", "ip", "link", "set", "dev", channel, "down"], check=True + ) + command = [ + "sudo", + "-n", + "ip", + "link", + "set", + "dev", + channel, + "type", + "can", + "bitrate", + str(bitrate), + ] + if fd: + command += ["dbitrate", str(dbitrate), "fd", "on"] + else: + command += ["fd", "off"] + subprocess.run(command, check=True) + subprocess.run( + ["sudo", "-n", "ip", "link", "set", "dev", channel, "up"], check=True + ) + + +def switch(bus, request_id, source_fd, target_fd, side, *, timeout=0.5, save=False): + configuration, wire = inspect(bus, request_id, source_fd, side, timeout) + if wire != source_fd: + raise RuntimeError("Actual reply format differs from --from-mode") + target = 2 if target_fd else 1 + if not target_fd and configuration[1]: + raise RuntimeError("BRS must be disabled before switching to classic CAN") + if configuration[0] != target or wire != target_fd: + bus.send( + can.Message( + arbitration_id=request_id, + data=bytes((0x35, 6, 1, target)), + is_extended_id=False, + is_fd=source_fd, + check=True, + ), + timeout=timeout, + ) + print( + "Mode write sent once; an absent reply does not imply no change", flush=True + ) + updated, actual_fd = inspect(bus, request_id, target_fd, side, timeout) + if updated[0] != target or actual_fd != target_fd: + raise RuntimeError("Target mode or active reply format was not verified") + if save: + result, _ = exchange( + bus, + request_id, + bytes.fromhex("351E0101"), + can_fd=target_fd, + timeout=timeout, + response_fd=target_fd, + ) + if result != b"\x00": + raise RuntimeError( + "Save completion not confirmed; do not repeat automatically" + ) + print( + "Flash save completion confirmed; power-cycle persistence not tested", + flush=True, + ) + print("Target configuration and active wire format verified", flush=True) + + +def main(argv=None): + parser = argparse.ArgumentParser(description=__doc__) + parser.add_argument("--channel", default="can0") + parser.add_argument("--request-id", type=lambda value: int(value, 0), default=1) + parser.add_argument("--side", choices=("left", "right"), required=True) + parser.add_argument("--from-mode", choices=("can", "canfd"), required=True) + parser.add_argument("--mode", choices=("can", "canfd")) + parser.add_argument( + "--save", + action="store_true", + help="Explicitly save the verified target to Flash", + ) + parser.add_argument("--bitrate", type=int, default=1_000_000) + parser.add_argument("--dbitrate", type=int, default=5_000_000) + parser.add_argument("--timeout", type=float, default=0.5) + args = parser.parse_args(argv) + if not 0 <= args.request_id <= 0x3FE: + parser.error("request ID must be within 0..0x3FE") + if not 0 < args.bitrate <= 1_000_000 or not 0 < args.dbitrate <= 8_000_000: + parser.error("invalid bitrates") + if not math.isfinite(args.timeout) or args.timeout <= 0: + parser.error("timeout must be finite and positive") + if args.save and args.mode is None: + parser.error("--save requires --mode") + configure_host(args.channel, True, args.bitrate, args.dbitrate) + with can.Bus(interface="socketcan", channel=args.channel, fd=True) as bus: + if args.mode is None: + inspect( + bus, args.request_id, args.from_mode == "canfd", args.side, args.timeout + ) + else: + switch( + bus, + args.request_id, + args.from_mode == "canfd", + args.mode == "canfd", + args.side, + timeout=args.timeout, + save=args.save, + ) + if args.mode is not None: + configure_host(args.channel, args.mode == "canfd", args.bitrate, args.dbitrate) + + +if __name__ == "__main__": + main() diff --git a/src/linkerbot_hand/__init__.py b/src/linkerbot_hand/__init__.py index 8e68d96..cafcbfc 100644 --- a/src/linkerbot_hand/__init__.py +++ b/src/linkerbot_hand/__init__.py @@ -4,6 +4,7 @@ AckSupport, AngleFeedbackPolicy, AngleSample, + CANConfiguration, ChannelMetrics, ClosedError, CommandFamily, @@ -54,6 +55,7 @@ PositionKdSample, PositionKpSample, ProtocolError, + ProtocolKind, Quantization, QueueFullError, RawNumber, @@ -138,6 +140,7 @@ "AmbiguousHandError", "AngleFeedbackPolicy", "AngleSample", + "CANConfiguration", "ChannelMetrics", "ClosedError", "CommandCapability", @@ -217,6 +220,7 @@ "PositionKdSample", "PositionKpSample", "ProtocolError", + "ProtocolKind", "PythonCANDiscoveryConfig", "Quantization", "QueueFullError", diff --git a/src/linkerbot_hand/api/__init__.py b/src/linkerbot_hand/api/__init__.py index cd2bbfd..d1e64f5 100644 --- a/src/linkerbot_hand/api/__init__.py +++ b/src/linkerbot_hand/api/__init__.py @@ -1,5 +1,6 @@ """Stable public contracts for the Hand SDK architecture.""" +from .can_configuration import CANConfiguration from .commands import ( AckSupport, CommandFamily, @@ -16,6 +17,7 @@ DataOrigin, HandSide, JointMotionType, + ProtocolKind, Quantization, ReadMode, SampleProjection, @@ -95,6 +97,7 @@ "AckSupport", "AngleFeedbackPolicy", "AngleSample", + "CANConfiguration", "ChannelMetrics", "ClosedError", "CommandFamily", @@ -145,6 +148,7 @@ "PositionKdSample", "PositionKpSample", "ProtocolError", + "ProtocolKind", "Quantization", "QueueFullError", "RawNumber", diff --git a/src/linkerbot_hand/api/_device_core.py b/src/linkerbot_hand/api/_device_core.py index 73c8d0c..afdd678 100644 --- a/src/linkerbot_hand/api/_device_core.py +++ b/src/linkerbot_hand/api/_device_core.py @@ -20,6 +20,7 @@ from .commands import CommandHandle from .config import DeviceConfig, PollingPlan from .device_info import DeviceInfo +from .enums import ProtocolKind, TransportKind from .errors import ( ClosedError, LifecycleError, @@ -83,6 +84,7 @@ def __init__( model: str, transport: object, side: str | None = None, + protocol: ProtocolKind | str | None = None, config: DeviceConfig | None = None, clock: object | None = None, protocol_backend: object | None = None, @@ -99,9 +101,20 @@ def __init__( if runtime_factory is not None and not callable(runtime_factory): raise ValidationError("runtime_factory must be callable or None") - capabilities = get_declared_capabilities(model, side=side) self._transport = transport self._config = DeviceConfig() if config is None else config + capabilities = get_declared_capabilities(model, side=side, protocol=protocol) + if self._config.hop_transport is TransportKind.CAN: + if ( + capabilities.model not in {"O6", "O30"} + or capabilities.protocol is not ProtocolKind.HOP + ): + raise ValidationError( + "classic HOP CAN requires O6 or O30 with protocol='hop'" + ) + capabilities = get_declared_capabilities( + model, side=side, protocol=protocol, transport_kind=TransportKind.CAN + ) if self._config.o30_request_id is not None: if capabilities.model != "O30": raise ValidationError("o30_request_id is only valid for O30") @@ -109,6 +122,13 @@ def __init__( raise ValidationError( "use o30_request_id or protocol_backend, not both" ) + if self._config.hop_request_id is not None or self._config.hop_brs: + if capabilities.protocol is not ProtocolKind.HOP: + raise ValidationError("HOP configuration requires protocol='hop'") + if protocol_backend is not None: + raise ValidationError( + "use HOP configuration or protocol_backend, not both" + ) self._clock = clock self._protocol_backend = protocol_backend self._runtime_factory = ( diff --git a/src/linkerbot_hand/api/_runtime_contract.py b/src/linkerbot_hand/api/_runtime_contract.py index 37b7b76..356466b 100644 --- a/src/linkerbot_hand/api/_runtime_contract.py +++ b/src/linkerbot_hand/api/_runtime_contract.py @@ -9,6 +9,7 @@ DeviceCapabilities, TelemetrySemantic, ) +from .can_configuration import CANConfiguration from .commands import CommandHandle from .config import DeviceConfig, PollingPlan from .device_info import DeviceInfo @@ -37,6 +38,12 @@ def read(self, semantic: TelemetrySemantic, timeout: float) -> object: ... def read_device_info(self, timeout: float) -> DeviceInfo: ... + def read_can_configuration(self, timeout: float) -> CANConfiguration: ... + + def write_can_configuration( + self, sub_index: int, payload: bytes, timeout: float + ) -> None: ... + def operate(self, operation: str) -> CommandHandle: ... def recover(self, timeout: float) -> None: ... diff --git a/src/linkerbot_hand/api/can_configuration.py b/src/linkerbot_hand/api/can_configuration.py new file mode 100644 index 0000000..885f59d --- /dev/null +++ b/src/linkerbot_hand/api/can_configuration.py @@ -0,0 +1,73 @@ +"""HOP device-side CAN configuration, independent of adapter settings.""" + +from dataclasses import dataclass + +from .errors import ValidationError + +NOMINAL_BITRATES = ( + 1_000_000, + 800_000, + 500_000, + 400_000, + 250_000, + 200_000, + 125_000, + 100_000, +) +DATA_BITRATES = ( + 8_000_000, + 5_000_000, + 4_000_000, + 3_000_000, + 2_000_000, + 1_000_000, + 800_000, + 500_000, + 400_000, + 250_000, + 200_000, + 125_000, + 100_000, +) + + +@dataclass(frozen=True, slots=True) +class CANConfiguration: + """Device register values; these do not reconfigure the host adapter.""" + + request_id: int + interface_type: int + brs: bool + nominal_bitrate: int + data_bitrate: int + + def __post_init__(self) -> None: + validate_can_id(self.request_id) + if type(self.interface_type) is not int or self.interface_type not in (1, 2): + raise ValidationError("CAN interface_type must be 1 or 2") + if type(self.brs) is not bool: + raise ValidationError("brs must be bool") + bitrate_code(self.nominal_bitrate, data=False) + bitrate_code(self.data_bitrate, data=True) + + @property + def response_id(self) -> int: + """Return the derived standard response ID.""" + return self.request_id | 0x400 + + +def validate_can_id(value: int) -> None: + if type(value) is not int or not 0 <= value <= 0x3FE: + raise ValidationError("CAN request ID must be int in 0x000..0x3FE") + + +def bitrate_code(value: int, *, data: bool) -> int: + allowed = DATA_BITRATES if data else NOMINAL_BITRATES + if type(value) is not int or value not in allowed: + raise ValidationError( + f"unsupported {'data' if data else 'nominal'} bitrate: {value!r}" + ) + return allowed.index(value) + 1 + + +__all__ = ["CANConfiguration"] diff --git a/src/linkerbot_hand/api/config.py b/src/linkerbot_hand/api/config.py index 321ad93..fc16256 100644 --- a/src/linkerbot_hand/api/config.py +++ b/src/linkerbot_hand/api/config.py @@ -5,7 +5,7 @@ import math from dataclasses import dataclass, fields -from .enums import AngleFeedbackPolicy +from .enums import AngleFeedbackPolicy, TransportKind from .errors import ValidationError @@ -76,7 +76,13 @@ def enabled_rates(self) -> tuple[tuple[str, float], ...]: @dataclass(frozen=True, slots=True) class DeviceConfig: - """Runtime limits, optional O30 endpoint and recovery policy for one hand. + """Runtime limits, HOP endpoint parameters and recovery policy for one hand. + + ``hop_request_id`` selects a host-side standard HOP endpoint. ``hop_brs`` + defaults off and sets outgoing FD frame BRS; neither field writes device + registers. ``hop_transport`` selects O6/O30 HOP over classic CAN or CAN FD + (default); it does not switch device or adapter mode. ``o30_request_id`` is + retained as a compatible O30-only alias. ``identity_timeout_s`` bounds each identity response stage after its request batch has been sent, including every stage of L30/O30 identity chains. @@ -104,8 +110,31 @@ class DeviceConfig: recovery_timeout_s: float = 5.0 recovery_max_attempts: int = 3 recovery_retry_delay_s: float = 0.2 + hop_request_id: int | None = None + hop_brs: bool = False + hop_transport: TransportKind = TransportKind.CAN_FD def __post_init__(self) -> None: + if self.hop_transport not in ( + TransportKind.CAN, + TransportKind.CAN_FD, + ) or not isinstance(self.hop_transport, TransportKind): + raise ValidationError("hop_transport must be TransportKind.CAN or CAN_FD") + if self.hop_transport is TransportKind.CAN and self.hop_brs: + raise ValidationError("classic HOP CAN cannot enable BRS") + if self.hop_request_id is not None and ( + type(self.hop_request_id) is not int + or not 0 <= self.hop_request_id <= 0x3FE + ): + raise ValidationError("hop_request_id must be within 0x000..0x3FE or None") + if type(self.hop_brs) is not bool: + raise ValidationError("hop_brs must be bool") + if ( + self.hop_request_id is not None + and self.o30_request_id is not None + and self.hop_request_id != self.o30_request_id + ): + raise ValidationError("hop_request_id disagrees with o30_request_id") if self.o30_request_id is not None and ( type(self.o30_request_id) is not int or not 0 <= self.o30_request_id <= 0x3FE diff --git a/src/linkerbot_hand/api/device.py b/src/linkerbot_hand/api/device.py index eb59d8a..060d963 100644 --- a/src/linkerbot_hand/api/device.py +++ b/src/linkerbot_hand/api/device.py @@ -10,6 +10,7 @@ from ._device_core import _coerce_timeout, _DeviceCore from ._force import age_force_fields, merge_force_samples from ._force_names import DEFAULT_FORCE_FIELDS, FORCE_FIELDS, validate_force_field +from .can_configuration import CANConfiguration, bitrate_code, validate_can_id from .commands import CommandHandle from .device_info import DeviceInfo from .enums import SampleProjection @@ -192,7 +193,7 @@ def set_torques_limits(self, values: Iterable[RawNumber]) -> CommandHandle: ) def get_torques_limits(self, timeout: float) -> TorqueLimitSample: - """Read raw torque limit settings (O30 MI 0x06, RTS=1).""" + """Read raw torque limit settings (HOP MI 0x06, RTS=1).""" return self._read( TelemetrySemantic.TORQUES_LIMITS, @@ -219,7 +220,8 @@ def set_torques(self, values: Iterable[RawNumber]) -> CommandHandle: def get_torques(self, timeout: float) -> TorqueSample: """Query raw torque values with model-specific measurement semantics. - L6 reads 0x03 torque output/mode; measured physical torque is unverified. + Legacy L6 reads 0x03 output/mode; its physical torque is unverified. + HOP L6/O6 read MI 0x06 actual values without selecting control mode. O30 reads MI 0x06 measured torque. Consult the telemetry capability's device_measured and unit metadata before interpreting the raw values. """ @@ -402,7 +404,7 @@ def get_normal_force_resultant( self, timeout: float, ) -> NormalForceResultantSample: - """Read all five O30 fingertips and return their raw ``FnS`` values.""" + """Read five descriptor-based HOP fingertips and return raw ``FnS`` values.""" operation = "get_normal_force_resultant" # The resultant comes from the descriptor tactile projection. Ask the @@ -558,6 +560,63 @@ def get_limit_compensations(self, timeout: float) -> LimitCompensationSample: operation="get_limit_compensations", ) + def get_can_configuration(self, timeout: float = 1.0) -> CANConfiguration: + """Query L6/O6 HOP device registers; does not change adapter settings.""" + if not self.get_capabilities().operations.read_can_configuration: + self._raise_unsupported("get_can_configuration") + return self._require_runtime("get_can_configuration").read_can_configuration( + _coerce_timeout(timeout) + ) + + def set_can_id(self, request_id: int, timeout: float = 1.0) -> None: + """Send a new standard request ID, then close; stop polling first. + + Success means dispatched, not verified applied or persisted. Construct a + new device with DeviceConfig(hop_request_id=...) to reconnect and verify. + """ + validate_can_id(request_id) + self._write_can_configuration(0, request_id.to_bytes(2, "little"), timeout) + + def set_can_brs(self, enabled: bool, timeout: float = 1.0) -> None: + """Send device-side BRS on/off, then close; stop polling first.""" + if type(enabled) is not bool: + raise ValidationError("enabled must be bool") + self._write_can_configuration(7, bytes([int(enabled)]), timeout) + + def set_can_bitrates( + self, + *, + nominal_bitrate: int | None = None, + data_bitrate: int | None = None, + timeout: float = 1.0, + ) -> None: + """Send supported device bitrate codes, then close; stop polling first. + + Reconfigure the host adapter separately before reconnecting. If both + values are provided they are written in one HOP frame. + """ + if nominal_bitrate is None and data_bitrate is None: + raise ValidationError("provide nominal_bitrate or data_bitrate") + payload = b"" + if nominal_bitrate is not None: + payload += bytes([bitrate_code(nominal_bitrate, data=False)]) + if data_bitrate is not None: + payload += bytes([bitrate_code(data_bitrate, data=True)]) + self._write_can_configuration( + 8 if nominal_bitrate is not None else 9, payload, timeout + ) + + def _write_can_configuration( + self, sub_index: int, payload: bytes, timeout: float + ) -> None: + timeout = _coerce_timeout(timeout) + if not self.get_capabilities().operations.write_can_configuration: + self._raise_unsupported("set_can_configuration") + with self._transition_lock: + self._require_runtime("set_can_configuration").write_can_configuration( + sub_index, payload, timeout + ) + def clear_faults(self) -> CommandHandle: """Submit one bounded discrete clear-fault operation.""" diff --git a/src/linkerbot_hand/api/enums.py b/src/linkerbot_hand/api/enums.py index 9c83ea4..9b2ef63 100644 --- a/src/linkerbot_hand/api/enums.py +++ b/src/linkerbot_hand/api/enums.py @@ -11,6 +11,13 @@ class HandSide(str, Enum): NOT_APPLICABLE = "not_applicable" +class ProtocolKind(str, Enum): + """Application protocol, independent of the physical CAN frame family.""" + + LEGACY = "legacy" + HOP = "hop" + + class DataOrigin(str, Enum): """How a cached sample entered the canonical snapshot.""" @@ -97,6 +104,7 @@ class Quantization(str, Enum): "DataOrigin", "HandSide", "JointMotionType", + "ProtocolKind", "Quantization", "ReadMode", "SampleProjection", diff --git a/src/linkerbot_hand/api/readings.py b/src/linkerbot_hand/api/readings.py index d6cf1a7..16a4874 100644 --- a/src/linkerbot_hand/api/readings.py +++ b/src/linkerbot_hand/api/readings.py @@ -680,7 +680,7 @@ def available(self) -> bool: @dataclass(frozen=True, slots=True) class TactileSensorSample: - """One O30 sensor's exact bytes and descriptor-decoded values.""" + """One HOP sensor's exact bytes and descriptor-decoded values.""" metadata: TactileSensorMetadata raw_data: bytes diff --git a/src/linkerbot_hand/capabilities/models/_hop.py b/src/linkerbot_hand/capabilities/models/_hop.py new file mode 100644 index 0000000..f6ae403 --- /dev/null +++ b/src/linkerbot_hand/capabilities/models/_hop.py @@ -0,0 +1,367 @@ +"""Shared HOP object capabilities, parameterized by installed joint layout.""" + +from ...api.commands import AckSupport +from ...api.enums import ( + Confidence, + HandSide, + ProtocolKind, + SampleProjection, + TransportKind, +) +from ..types import ( + CommandSemantic, + DeviceCapabilities, + JointCapability, + OperationCapabilities, + StallFieldCapability, + StallProtectionCapability, + TelemetrySemantic, + TransportCapability, +) +from ._common import ( + U8_RANGE, + angle_command, + angle_telemetry, + command, + identity, + telemetry, +) + +_SENSOR_CHANNELS = ("thumb", "index", "middle", "ring", "pinky") + + +def shared_capabilities( + model: str, + side: HandSide, + joints: tuple[JointCapability, ...], + *, + raw_encoding: str, + addressing_note: str, +) -> DeviceCapabilities: + """Declare common HOP objects; model profiles select their extra operations.""" + names = tuple(value.name for value in joints) + stall_field = StallFieldCapability( + raw_range=U8_RANGE, + raw_encoding=raw_encoding, + raw_unit=None, + raw_scale_note="only installed public channels are represented", + unit_confidence=Confidence.UNKNOWN, + ) + stall = StallProtectionCapability( + channels=names, + detection_time=stall_field, + threshold=stall_field, + hold_current=stall_field, + physical_semantics_confidence=Confidence.UNKNOWN, + safety_range_verified=False, + read_supported=True, + write_supported=True, + ) + return DeviceCapabilities( + protocol=ProtocolKind.HOP, + identity=identity( + model, + side, + supported_fields=( + "serial_number", + "pcb_version", + "adapter_board_version", + "control_board_version", + "bootloader_version", + "firmware_version", + "mechanical_version", + "product_code", + "protocol_name", + "protocol_version", + "power_supply_range", + "microcontroller_uid", + "build_time", + "supported_interfaces", + "reported_side", + ), + ), + joints=joints, + commands=( + angle_command( + joints, + raw_encoding, + ack_support=AckSupport.SUPPORTED, + returns_current_state=True, + ), + command( + CommandSemantic.SPEEDS, + names, + U8_RANGE, + raw_encoding, + raw_unit="count", + unit_confidence=Confidence.UNKNOWN, + ack_support=AckSupport.SUPPORTED, + ), + command( + CommandSemantic.ACCELERATIONS, + names, + U8_RANGE, + raw_encoding, + raw_unit="count", + unit_confidence=Confidence.UNKNOWN, + ack_support=AckSupport.SUPPORTED, + ), + command( + CommandSemantic.TORQUES_LIMITS, + names, + U8_RANGE, + raw_encoding, + raw_unit=None, + unit_confidence=Confidence.UNKNOWN, + ack_support=AckSupport.SUPPORTED, + ), + command( + CommandSemantic.CURRENT_LIMITS, + names, + U8_RANGE, + raw_encoding, + raw_unit="count", + unit_confidence=Confidence.UNKNOWN, + ack_support=AckSupport.SUPPORTED, + ), + command( + CommandSemantic.MOTION_TIMES, + names, + U8_RANGE, + raw_encoding, + raw_unit="count", + raw_scale_note="documentation states 10 ms/count; API returns count", + unit_confidence=Confidence.DOCUMENTED, + ack_support=AckSupport.SUPPORTED, + ), + command( + CommandSemantic.STALL_DETECTION_TIMES, + names, + U8_RANGE, + raw_encoding, + raw_unit=None, + unit_confidence=Confidence.UNKNOWN, + ack_support=AckSupport.SUPPORTED, + ), + command( + CommandSemantic.STALL_THRESHOLDS, + names, + U8_RANGE, + raw_encoding, + raw_unit=None, + unit_confidence=Confidence.UNKNOWN, + ack_support=AckSupport.SUPPORTED, + ), + command( + CommandSemantic.STALL_HOLD_CURRENTS, + names, + U8_RANGE, + raw_encoding, + raw_unit=None, + unit_confidence=Confidence.UNKNOWN, + ack_support=AckSupport.SUPPORTED, + ), + ), + telemetry=( + angle_telemetry( + joints, + raw_encoding, + recommended_hz=30, + estimated_transaction_s=0.004, + ), + telemetry( + TelemetrySemantic.MEASURED_SPEEDS, + names, + raw_encoding, + shape=(len(joints),), + raw_range=U8_RANGE, + raw_unit="count", + unit_confidence=Confidence.UNKNOWN, + recommended_hz=30, + stale_after_s=0.1, + estimated_transaction_s=0.004, + ), + telemetry( + TelemetrySemantic.CONFIGURED_SPEEDS, + names, + raw_encoding, + shape=(len(joints),), + raw_range=U8_RANGE, + raw_unit="count", + unit_confidence=Confidence.UNKNOWN, + recommended_hz=30, + stale_after_s=0.1, + estimated_transaction_s=0.004, + device_measured=False, + ), + telemetry( + TelemetrySemantic.CONFIGURED_ACCELERATIONS, + names, + raw_encoding, + shape=(len(joints),), + raw_range=U8_RANGE, + raw_unit="count", + unit_confidence=Confidence.UNKNOWN, + recommended_hz=10, + stale_after_s=0.3, + estimated_transaction_s=0.004, + device_measured=False, + ), + telemetry( + TelemetrySemantic.TORQUES_LIMITS, + names, + raw_encoding, + shape=(len(joints),), + raw_range=U8_RANGE, + raw_unit=None, + unit_confidence=Confidence.UNKNOWN, + recommended_hz=30, + stale_after_s=0.1, + estimated_transaction_s=0.004, + device_measured=False, + ), + telemetry( + TelemetrySemantic.TEMPERATURES, + names, + raw_encoding, + shape=(len(joints),), + raw_range=U8_RANGE, + raw_unit="count", + unit_confidence=Confidence.UNKNOWN, + recommended_hz=1, + stale_after_s=3, + estimated_transaction_s=0.004, + ), + telemetry( + TelemetrySemantic.FORCE, + _SENSOR_CHANNELS, + "o30_tactile_descriptor", + shape=(5,), + raw_range=None, + raw_unit=None, + raw_scale_note=( + "runtime metadata supplies sensor type, unit, shape, descriptor, " + "and exact raw byte stream; no physical conversion" + ), + unit_confidence=Confidence.DOCUMENTED, + recommended_hz=10, + stale_after_s=0.3, + estimated_transaction_s=0.2, + payload_projection=SampleProjection.TACTILE_DESCRIPTOR, + ), + telemetry( + TelemetrySemantic.CURRENTS, + names, + raw_encoding, + shape=(len(joints),), + raw_range=U8_RANGE, + raw_unit="count", + unit_confidence=Confidence.UNKNOWN, + recommended_hz=10, + stale_after_s=0.3, + estimated_transaction_s=0.004, + ), + telemetry( + TelemetrySemantic.CURRENT_LIMITS, + names, + raw_encoding, + shape=(len(joints),), + raw_range=U8_RANGE, + raw_unit="count", + unit_confidence=Confidence.UNKNOWN, + recommended_hz=30, + stale_after_s=0.1, + estimated_transaction_s=0.004, + device_measured=False, + ), + telemetry( + TelemetrySemantic.TORQUES, + names, + raw_encoding, + shape=(len(joints),), + raw_range=U8_RANGE, + raw_unit=None, + unit_confidence=Confidence.UNKNOWN, + recommended_hz=10, + stale_after_s=0.3, + estimated_transaction_s=0.004, + ), + telemetry( + TelemetrySemantic.FAULTS, + names, + "u8_bitmask_v25_object", + shape=(len(joints),), + raw_range=U8_RANGE, + raw_unit=None, + unit_confidence=Confidence.DOCUMENTED, + recommended_hz=5, + stale_after_s=0.5, + estimated_transaction_s=0.004, + # ``protocols/o30/telemetry.py`` wires decode_fault_statuses for + # this object, so every fault sample carries per-joint + # JointFaultStatus values next to the raw bytes. + decoded_statuses=True, + ), + telemetry( + TelemetrySemantic.VOLTAGES, + names, + raw_encoding, + shape=(len(joints),), + raw_range=U8_RANGE, + raw_unit="count", + unit_confidence=Confidence.UNKNOWN, + recommended_hz=1, + stale_after_s=3, + estimated_transaction_s=0.004, + ), + telemetry( + TelemetrySemantic.MOTION_TIMES, + names, + raw_encoding, + shape=(len(joints),), + raw_range=U8_RANGE, + raw_unit="count", + raw_scale_note="documentation states 10 ms/count; API returns count", + unit_confidence=Confidence.DOCUMENTED, + recommended_hz=10, + stale_after_s=0.3, + estimated_transaction_s=0.004, + device_measured=False, + ), + *( + telemetry( + semantic, + names, + raw_encoding, + shape=(len(joints),), + raw_range=U8_RANGE, + raw_unit=None, + unit_confidence=Confidence.UNKNOWN, + recommended_hz=1, + stale_after_s=3, + estimated_transaction_s=0.004, + device_measured=False, + ) + for semantic in ( + TelemetrySemantic.STALL_DETECTION_TIMES, + TelemetrySemantic.STALL_THRESHOLDS, + TelemetrySemantic.STALL_HOLD_CURRENTS, + ) + ), + ), + stall_protection=stall, + transport=TransportCapability( + kind=TransportKind.CAN_FD, + nominal_bitrate=1_000_000, + # BRS is disabled, so the data phase always runs at the 1 Mbit + # nominal rate and the interface's configured data bitrate is + # irrelevant. Leaving it unconstrained lets any dbitrate (1M/5M/7M…) + # connect and be discovered. + data_bitrate=None, + addressing_note=addressing_note, + ), + operations=OperationCapabilities( + read_stall_protection=True, + write_stall_protection=True, + ), + ) diff --git a/src/linkerbot_hand/capabilities/models/hop_six.py b/src/linkerbot_hand/capabilities/models/hop_six.py new file mode 100644 index 0000000..3abbcc5 --- /dev/null +++ b/src/linkerbot_hand/capabilities/models/hop_six.py @@ -0,0 +1,156 @@ +"""L6/O6 HOP profiles: shared objects, six installed logical positions.""" + +from dataclasses import replace + +from ...api.commands import AckSupport +from ...api.enums import Confidence, HandSide +from ..types import ( + CommandSemantic, + DeviceCapabilities, + OperationCapabilities, + TelemetrySemantic, +) +from ._common import U8_RANGE, angle_command, angle_telemetry, command, telemetry +from ._hop import shared_capabilities +from .l6 import declared_capabilities as legacy_l6 +from .o6 import declared_capabilities as legacy_o6 + + +def declared_capabilities(model: str, side: HandSide) -> DeviceCapabilities: + """Keep public six-joint order; never copy legacy scales or fault bits.""" + legacy = (legacy_l6 if model == "L6" else legacy_o6)(side) + joints = tuple( + replace( + j, + percent_mapping=replace( + j.percent_mapping, confidence=Confidence.APPROXIMATE + ), + ) + if j.percent_mapping is not None + else j + for j in legacy.joints + ) + template = shared_capabilities( + model, + side, + joints, + raw_encoding="u8_hop_v36", + addressing_note="HOP default standard request ID 1 for both sides; response=request|0x400; BRS off", + ) + names = legacy.joint_names + note = ( + "HOP v0.0.4 provisional raw 0..255; physical scale unverified; " + "six public joints occupy logical positions 10,5,11,12,13,14" + ) + commands = tuple( + replace( + c, + channels=names, + raw_encoding="u8_hop_v36", + raw_scale_note=note, + unit_confidence=Confidence.UNKNOWN, + ) + for c in template.commands + if c.semantic is not CommandSemantic.ANGLES + ) + channels = tuple( + replace( + t, + channels=names, + shape=(6,), + raw_encoding="u8_hop_v36", + raw_scale_note=note, + raw_unit=None, + unit_confidence=Confidence.UNKNOWN, + estimated_transaction_s=0.008, + ) + if t.semantic is not TelemetrySemantic.FORCE + else replace(t, raw_encoding="hop_tactile_descriptor") + for t in template.telemetry + if t.semantic is not TelemetrySemantic.ANGLES + ) + if model == "L6": + for cs, ts in ( + (CommandSemantic.POSITION_KP, TelemetrySemantic.POSITION_KP), + (CommandSemantic.POSITION_KD, TelemetrySemantic.POSITION_KD), + ): + commands += ( + command( + cs, + names, + U8_RANGE, + "u16_le_hop_v36", + raw_unit=None, + raw_scale_note=note, + unit_confidence=Confidence.UNKNOWN, + ack_support=AckSupport.SUPPORTED, + ), + ) + channels += ( + telemetry( + ts, + names, + "u16_le_hop_v36", + shape=(6,), + raw_range=U8_RANGE, + raw_unit=None, + raw_scale_note=note, + unit_confidence=Confidence.UNKNOWN, + recommended_hz=1, + stale_after_s=3, + estimated_transaction_s=0.008, + device_measured=False, + ), + ) + commands += ( + command( + CommandSemantic.TORQUES, + names, + U8_RANGE, + "u8_hop_v36", + raw_unit=None, + raw_scale_note="HOP MI 0x06 output/limit; raw 0 does not select position mode", + unit_confidence=Confidence.UNKNOWN, + ack_support=AckSupport.SUPPORTED, + ), + ) + assert template.stall_protection is not None + return replace( + template, + identity=replace(template.identity, model=model), + joints=joints, + commands=( + angle_command( + joints, + "u8_hop_v36", + ack_support=AckSupport.SUPPORTED, + returns_current_state=True, + ), + *commands, + ), + telemetry=( + angle_telemetry( + joints, "u8_hop_v36", recommended_hz=30, estimated_transaction_s=0.008 + ), + *channels, + ), + stall_protection=replace(template.stall_protection, channels=names), + transport=replace( + template.transport, + addressing_note=( + "HOP CAN FD standard request ID defaults to 1 for both sides; " + "response=request|0x400; side comes from identity; BRS defaults off" + ), + ), + operations=OperationCapabilities( + save_parameters=True, + read_stall_protection=True, + write_stall_protection=True, + acknowledged_operations=("save_parameters",), + read_can_configuration=True, + write_can_configuration=True, + ), + ) + + +__all__ = ["declared_capabilities"] diff --git a/src/linkerbot_hand/capabilities/models/o30.py b/src/linkerbot_hand/capabilities/models/o30.py index 5be1d32..ea5c1f2 100644 --- a/src/linkerbot_hand/capabilities/models/o30.py +++ b/src/linkerbot_hand/capabilities/models/o30.py @@ -1,30 +1,15 @@ """Declared O30 profile exposing only twenty installed protocol slots.""" -from ...api.commands import AckSupport from ...api.enums import ( - Confidence, HandSide, JointMotionType, - SampleProjection, - TransportKind, ) from ..types import ( - CommandSemantic, DeviceCapabilities, - OperationCapabilities, - StallFieldCapability, - StallProtectionCapability, - TelemetrySemantic, - TransportCapability, ) from ._common import ( U8_RANGE, - angle_command, - angle_telemetry, - command, - identity, joint, - telemetry, ) _RAW_ENCODING = "u8_v25_object" @@ -82,334 +67,17 @@ def declared_capabilities(side: HandSide) -> DeviceCapabilities: _JOINT_SPECS ) ) - names = tuple(value.name for value in joints) - stall_field = StallFieldCapability( - raw_range=U8_RANGE, + from ._hop import shared_capabilities + + return shared_capabilities( + "O30", + side, + joints, raw_encoding=_RAW_ENCODING, - raw_unit=None, - raw_scale_note="public 20 channels map to installed slots in V25", - unit_confidence=Confidence.UNKNOWN, - ) - stall = StallProtectionCapability( - channels=names, - detection_time=stall_field, - threshold=stall_field, - hold_current=stall_field, - physical_semantics_confidence=Confidence.UNKNOWN, - safety_range_verified=False, - read_supported=True, - write_supported=True, - ) - return DeviceCapabilities( - identity=identity( - "O30", - side, - supported_fields=( - "serial_number", - "pcb_version", - "adapter_board_version", - "control_board_version", - "bootloader_version", - "firmware_version", - "mechanical_version", - "product_code", - "protocol_name", - "protocol_version", - "power_supply_range", - "microcontroller_uid", - "build_time", - "supported_interfaces", - "reported_side", - ), - ), - joints=joints, - commands=( - angle_command( - joints, - _RAW_ENCODING, - ack_support=AckSupport.SUPPORTED, - returns_current_state=True, - ), - command( - CommandSemantic.SPEEDS, - names, - U8_RANGE, - _RAW_ENCODING, - raw_unit="count", - unit_confidence=Confidence.UNKNOWN, - ack_support=AckSupport.SUPPORTED, - ), - command( - CommandSemantic.ACCELERATIONS, - names, - U8_RANGE, - _RAW_ENCODING, - raw_unit="count", - unit_confidence=Confidence.UNKNOWN, - ack_support=AckSupport.SUPPORTED, - ), - command( - CommandSemantic.TORQUES_LIMITS, - names, - U8_RANGE, - _RAW_ENCODING, - raw_unit=None, - unit_confidence=Confidence.UNKNOWN, - ack_support=AckSupport.SUPPORTED, - ), - command( - CommandSemantic.CURRENT_LIMITS, - names, - U8_RANGE, - _RAW_ENCODING, - raw_unit="count", - unit_confidence=Confidence.UNKNOWN, - ack_support=AckSupport.SUPPORTED, - ), - command( - CommandSemantic.MOTION_TIMES, - names, - U8_RANGE, - _RAW_ENCODING, - raw_unit="count", - raw_scale_note="documentation states 10 ms/count; API returns count", - unit_confidence=Confidence.DOCUMENTED, - ack_support=AckSupport.SUPPORTED, - ), - command( - CommandSemantic.STALL_DETECTION_TIMES, - names, - U8_RANGE, - _RAW_ENCODING, - raw_unit=None, - unit_confidence=Confidence.UNKNOWN, - ack_support=AckSupport.SUPPORTED, - ), - command( - CommandSemantic.STALL_THRESHOLDS, - names, - U8_RANGE, - _RAW_ENCODING, - raw_unit=None, - unit_confidence=Confidence.UNKNOWN, - ack_support=AckSupport.SUPPORTED, - ), - command( - CommandSemantic.STALL_HOLD_CURRENTS, - names, - U8_RANGE, - _RAW_ENCODING, - raw_unit=None, - unit_confidence=Confidence.UNKNOWN, - ack_support=AckSupport.SUPPORTED, - ), - ), - telemetry=( - angle_telemetry( - joints, - _RAW_ENCODING, - recommended_hz=30, - estimated_transaction_s=0.004, - ), - telemetry( - TelemetrySemantic.MEASURED_SPEEDS, - names, - _RAW_ENCODING, - shape=(20,), - raw_range=U8_RANGE, - raw_unit="count", - unit_confidence=Confidence.UNKNOWN, - recommended_hz=30, - stale_after_s=0.1, - estimated_transaction_s=0.004, - ), - telemetry( - TelemetrySemantic.CONFIGURED_SPEEDS, - names, - _RAW_ENCODING, - shape=(20,), - raw_range=U8_RANGE, - raw_unit="count", - unit_confidence=Confidence.UNKNOWN, - recommended_hz=30, - stale_after_s=0.1, - estimated_transaction_s=0.004, - device_measured=False, - ), - telemetry( - TelemetrySemantic.CONFIGURED_ACCELERATIONS, - names, - _RAW_ENCODING, - shape=(20,), - raw_range=U8_RANGE, - raw_unit="count", - unit_confidence=Confidence.UNKNOWN, - recommended_hz=10, - stale_after_s=0.3, - estimated_transaction_s=0.004, - device_measured=False, - ), - telemetry( - TelemetrySemantic.TORQUES_LIMITS, - names, - _RAW_ENCODING, - shape=(20,), - raw_range=U8_RANGE, - raw_unit=None, - unit_confidence=Confidence.UNKNOWN, - recommended_hz=30, - stale_after_s=0.1, - estimated_transaction_s=0.004, - device_measured=False, - ), - telemetry( - TelemetrySemantic.TEMPERATURES, - names, - _RAW_ENCODING, - shape=(20,), - raw_range=U8_RANGE, - raw_unit="count", - unit_confidence=Confidence.UNKNOWN, - recommended_hz=1, - stale_after_s=3, - estimated_transaction_s=0.004, - ), - telemetry( - TelemetrySemantic.FORCE, - _SENSOR_CHANNELS, - "o30_tactile_descriptor", - shape=(5,), - raw_range=None, - raw_unit=None, - raw_scale_note=( - "runtime metadata supplies sensor type, unit, shape, descriptor, " - "and exact raw byte stream; no physical conversion" - ), - unit_confidence=Confidence.DOCUMENTED, - recommended_hz=10, - stale_after_s=0.3, - estimated_transaction_s=0.2, - payload_projection=SampleProjection.TACTILE_DESCRIPTOR, - ), - telemetry( - TelemetrySemantic.CURRENTS, - names, - _RAW_ENCODING, - shape=(20,), - raw_range=U8_RANGE, - raw_unit="count", - unit_confidence=Confidence.UNKNOWN, - recommended_hz=10, - stale_after_s=0.3, - estimated_transaction_s=0.004, - ), - telemetry( - TelemetrySemantic.CURRENT_LIMITS, - names, - _RAW_ENCODING, - shape=(20,), - raw_range=U8_RANGE, - raw_unit="count", - unit_confidence=Confidence.UNKNOWN, - recommended_hz=30, - stale_after_s=0.1, - estimated_transaction_s=0.004, - device_measured=False, - ), - telemetry( - TelemetrySemantic.TORQUES, - names, - _RAW_ENCODING, - shape=(20,), - raw_range=U8_RANGE, - raw_unit=None, - unit_confidence=Confidence.UNKNOWN, - recommended_hz=10, - stale_after_s=0.3, - estimated_transaction_s=0.004, - ), - telemetry( - TelemetrySemantic.FAULTS, - names, - "u8_bitmask_v25_object", - shape=(20,), - raw_range=U8_RANGE, - raw_unit=None, - unit_confidence=Confidence.DOCUMENTED, - recommended_hz=5, - stale_after_s=0.5, - estimated_transaction_s=0.004, - # ``protocols/o30/telemetry.py`` wires decode_fault_statuses for - # this object, so every fault sample carries per-joint - # JointFaultStatus values next to the raw bytes. - decoded_statuses=True, - ), - telemetry( - TelemetrySemantic.VOLTAGES, - names, - _RAW_ENCODING, - shape=(20,), - raw_range=U8_RANGE, - raw_unit="count", - unit_confidence=Confidence.UNKNOWN, - recommended_hz=1, - stale_after_s=3, - estimated_transaction_s=0.004, - ), - telemetry( - TelemetrySemantic.MOTION_TIMES, - names, - _RAW_ENCODING, - shape=(20,), - raw_range=U8_RANGE, - raw_unit="count", - raw_scale_note="documentation states 10 ms/count; API returns count", - unit_confidence=Confidence.DOCUMENTED, - recommended_hz=10, - stale_after_s=0.3, - estimated_transaction_s=0.004, - device_measured=False, - ), - *( - telemetry( - semantic, - names, - _RAW_ENCODING, - shape=(20,), - raw_range=U8_RANGE, - raw_unit=None, - unit_confidence=Confidence.UNKNOWN, - recommended_hz=1, - stale_after_s=3, - estimated_transaction_s=0.004, - device_measured=False, - ) - for semantic in ( - TelemetrySemantic.STALL_DETECTION_TIMES, - TelemetrySemantic.STALL_THRESHOLDS, - TelemetrySemantic.STALL_HOLD_CURRENTS, - ) - ), - ), - stall_protection=stall, - transport=TransportCapability( - kind=TransportKind.CAN_FD, - nominal_bitrate=1_000_000, - # BRS is disabled, so the data phase always runs at the 1 Mbit - # nominal rate and the interface's configured data bitrate is - # irrelevant. Leaving it unconstrained lets any dbitrate (1M/5M/7M…) - # connect and be discovered. - data_bitrate=None, - addressing_note=( - "HOP default standard-frame CAN ID pair: right " - "hand request 0x001 / response 0x401, left hand request 0x002 / " - "response 0x402. CAN FD BRS disabled, so the data phase equals " - "the 1 Mbit nominal rate and any interface data bitrate works." - ), - ), - operations=OperationCapabilities( - read_stall_protection=True, - write_stall_protection=True, + addressing_note=( + "HOP default standard-frame CAN ID pair: right hand request " + "0x001 / response 0x401, left hand request 0x002 / response 0x402. " + "CAN FD BRS disabled; nominal bitrate 1 Mbit/s; any interface data bitrate." ), ) diff --git a/src/linkerbot_hand/capabilities/registry.py b/src/linkerbot_hand/capabilities/registry.py index 48d52aa..8067219 100644 --- a/src/linkerbot_hand/capabilities/registry.py +++ b/src/linkerbot_hand/capabilities/registry.py @@ -3,8 +3,9 @@ from __future__ import annotations from collections.abc import Callable +from dataclasses import replace -from ..api.enums import HandSide +from ..api.enums import HandSide, ProtocolKind, TransportKind from ..api.errors import ValidationError from .models import ( l6_capabilities, @@ -47,6 +48,8 @@ def get_declared_capabilities( model: str, *, side: HandSide | str | None = None, + protocol: ProtocolKind | str | None = None, + transport_kind: TransportKind | str | None = None, ) -> DeviceCapabilities: """Return zero-I/O capability metadata for a configured model. @@ -54,6 +57,8 @@ def get_declared_capabilities( model: Case-insensitive public model name, or ``l20-lite`` for L20Lite. Historical product-code prefixes are not configuration aliases. side: ``left`` or ``right`` for sided models. L30 requires omission. + protocol: ``legacy`` or ``hop``. L6/O6 default to legacy; O30 to HOP. + transport_kind: Optional wire mode; HOP O6/O30 support ``can`` and ``can_fd``. Returns: An immutable, unverified declared profile. @@ -78,7 +83,57 @@ def get_declared_capabilities( if side not in {HandSide.LEFT, HandSide.RIGHT}: raise ValidationError("side must be left or right") resolved_side = side - return _PROFILE_FACTORIES[canonical](resolved_side) + try: + selected = ( + ProtocolKind(protocol) + if protocol is not None + else (ProtocolKind.HOP if canonical == "O30" else ProtocolKind.LEGACY) + ) + except (ValueError, TypeError) as error: + raise ValidationError("protocol must be legacy or hop") from error + if selected is ProtocolKind.HOP and canonical in {"L6", "O6"}: + from .models.hop_six import declared_capabilities + + capabilities = declared_capabilities(canonical, resolved_side) + return _with_transport(capabilities, transport_kind) + expected = ProtocolKind.HOP if canonical == "O30" else ProtocolKind.LEGACY + if selected is not expected: + raise ValidationError(f"{canonical} does not support {selected.value}") + capabilities = _PROFILE_FACTORIES[canonical](resolved_side) + return _with_transport(capabilities, transport_kind) + + +def _with_transport( + capabilities: DeviceCapabilities, transport_kind: TransportKind | str | None +) -> DeviceCapabilities: + if transport_kind is None: + return capabilities + try: + kind = TransportKind(transport_kind) + except (ValueError, TypeError) as error: + raise ValidationError("transport_kind must be can or can_fd") from error + if ( + capabilities.model in {"O6", "O30"} + and capabilities.protocol is ProtocolKind.HOP + and kind is TransportKind.CAN + ): + return replace( + capabilities, + transport=replace( + capabilities.transport, + kind=kind, + data_bitrate=None, + addressing_note=( + "HOP configurable standard request ID; response=request|0x400. " + "O6 defaults to request ID 1 for both sides. " + "O30 defaults to right 1, left 2. Classic CAN, default 1 Mbit/s; " + "at most five payload bytes per frame; BRS unavailable." + ), + ), + ) + if kind is not capabilities.transport.kind: + raise ValidationError(f"{capabilities.model} does not support {kind.value}") + return capabilities def _canonical_model(model: object) -> str: diff --git a/src/linkerbot_hand/capabilities/types.py b/src/linkerbot_hand/capabilities/types.py index 9e29670..83c8d91 100644 --- a/src/linkerbot_hand/capabilities/types.py +++ b/src/linkerbot_hand/capabilities/types.py @@ -5,7 +5,7 @@ from dataclasses import dataclass from ..api._force_names import validate_force_field -from ..api.enums import HandSide, JointMotionType, TransportKind +from ..api.enums import HandSide, JointMotionType, ProtocolKind, TransportKind from ..api.errors import ValidationError from ..api.readings import TactileSensorMetadata from .force import ForceFieldCapability, force_field_capabilities @@ -178,9 +178,13 @@ class OperationCapabilities: read_stall_protection: bool = False write_stall_protection: bool = False acknowledged_operations: tuple[str, ...] = () + read_can_configuration: bool = False + write_can_configuration: bool = False def __post_init__(self) -> None: for name in ( + "read_can_configuration", + "write_can_configuration", "read_device_info", "clear_faults", "enable", @@ -223,8 +227,11 @@ class DeviceCapabilities: operations: OperationCapabilities firmware_unsupported_telemetry: tuple[TelemetrySemantic, ...] = () tactile_sensors: tuple[TactileSensorMetadata, ...] = () + protocol: ProtocolKind = ProtocolKind.LEGACY def __post_init__(self) -> None: + if not isinstance(self.protocol, ProtocolKind): + raise ValidationError("protocol must be ProtocolKind") if not isinstance(self.identity, IdentityCapability): raise ValidationError("identity must be IdentityCapability") if ( diff --git a/src/linkerbot_hand/discovery/_engine.py b/src/linkerbot_hand/discovery/_engine.py index 1d65e80..581175b 100644 --- a/src/linkerbot_hand/discovery/_engine.py +++ b/src/linkerbot_hand/discovery/_engine.py @@ -26,7 +26,8 @@ from linkerbot_transport import ValidationError as TransportValidationError from linkerbot_transport.canfd import CANFDMessage -from ..api.enums import HandSide, TransportKind +from ..api.can_configuration import NOMINAL_BITRATES +from ..api.enums import HandSide, ProtocolKind, TransportKind from ..api.errors import ( HandError, IdentityMismatchError, @@ -43,6 +44,19 @@ matching_data, parse_factory_identity, ) +from ..protocols.hop.framing import ( + HOPAddressing, + matching_response_frame, +) +from ..protocols.hop.framing import ( + build_read_message as build_hop_read, +) +from ..protocols.hop.framing import ( + response_route as hop_response_route, +) +from ..protocols.hop.identity import identity_transaction as hop_identity_transaction +from ..protocols.hop.objects import MI_PRODUCT_INFO +from ..protocols.hop.response import HOPRangeAssembler from ..protocols.l30.codec import PARENT_CONFIG, SUB_DEVICE_INFO, SUB_PRODUCT_CODE from ..protocols.l30.framing import build_request as build_l30_request from ..protocols.l30.framing import parse_can_id as parse_l30_can_id @@ -59,18 +73,6 @@ read_route as o20_read_route, ) from ..protocols.o20.identity import o20_identity_transaction -from ..protocols.o30.codec import MI_PRODUCT_INFO -from ..protocols.o30.framing import ( - HOPAddressing, - matching_response_frame, -) -from ..protocols.o30.framing import ( - build_read_message as build_o30_read, -) -from ..protocols.o30.framing import ( - response_route as o30_response_route, -) -from ..protocols.o30.identity import o30_identity_transaction from ..protocols.registry import create_protocol_backend from . import _budget from ._classic_routes import ( @@ -261,7 +263,10 @@ def _probe_channel( if _budget.expired(): break failed = False - if wire is TransportKind.CAN and _channel_matches_classic(channel): + if wire is TransportKind.CAN and ( + channel.nominal_bitrate is None + or channel.nominal_bitrate in NOMINAL_BITRATES + ): failed = _probe_classic(source, channel, config, candidates, failures) elif wire is TransportKind.CAN_FD: failed = _probe_can_fd(source, channel, config, candidates, failures) @@ -290,6 +295,8 @@ def _probe_classic( mode_initialization = False for attempt, request_route in enumerate( (CLASSIC_BROADCAST_ROUTE, *CLASSIC_DEFAULT_ROUTES) + if _channel_matches_classic(channel) + else () ): routes = CLASSIC_DISCOVERY_ROUTES if attempt == 0 else (request_route,) messages = _collect( @@ -323,6 +330,13 @@ def _probe_classic( candidates.append( _verify_classic_candidate(transport, product, config, failures) ) + if not any( + c.channel == channel and c.status is DiscoveryStatus.VERIFIED + for c in candidates + ): + _probe_hop( + source, channel, transport, config, candidates, failures, can_fd=False + ) except Exception as error: # noqa: BLE001 - discovery source boundary # A mode-specific construction/open failure is isolated from the next # explicitly requested wire mode. Once the mode is open, a dead link @@ -538,8 +552,12 @@ def _probe_can_fd( ) -> bool: compatible_o20 = _channel_matches_model(channel, "O20", HandSide.LEFT) compatible_l30 = _channel_matches_model(channel, "L30", None) - compatible_o30 = _channel_matches_model(channel, "O30", HandSide.LEFT) - if not any((compatible_o20, compatible_l30, compatible_o30)): + # HOP device bitrates are configurable; probe at the adapter's already + # selected timing without retuning it or assuming the factory default. + compatible_hop = ( + channel.nominal_bitrate is None or channel.nominal_bitrate in NOMINAL_BITRATES + ) + if not any((compatible_o20, compatible_l30, compatible_hop)): return False transport: object | None = None failed = False @@ -556,9 +574,9 @@ def _probe_can_fd( lambda: _probe_o20(source, channel, transport, config, candidates), ), ( - "o30", - compatible_o30, - lambda: _probe_o30( + "o30", # Preserve the existing pre-identity diagnostic operation tag. + compatible_hop, + lambda: _probe_hop( source, channel, transport, config, candidates, failures ), ), @@ -931,18 +949,30 @@ def _probe_l30( ) -def _probe_o30( +def _probe_hop( source: DiscoverySource, channel: DiscoveryChannel, transport: object, config: DiscoveryConfig, candidates: list[DiscoveryCandidate], failures: list[DiscoveryFailure], + *, + can_fd: bool = True, ) -> None: # Defaults cover both sides; custom configured IDs do not encode a side. - for request_id in config.o30_request_ids: - _probe_o30_endpoint( - HOPAddressing(request_id, request_id | 0x400), + for request_id in ( + config.hop_request_ids + if config.hop_request_ids is not None + else config.o30_request_ids + ): + start = len(candidates) + _probe_hop_endpoint( + HOPAddressing( + request_id, + request_id | 0x400, + config.hop_brs if can_fd else False, + can_fd, + ), source, channel, transport, @@ -951,8 +981,13 @@ def _probe_o30( failures, ) + for index in range(start, len(candidates)): + candidates[index] = replace( + candidates[index], hop_brs=config.hop_brs if can_fd else False + ) + -def _probe_o30_endpoint( +def _probe_hop_endpoint( addressing: HOPAddressing, source: DiscoverySource, channel: DiscoveryChannel, @@ -961,21 +996,28 @@ def _probe_o30_endpoint( candidates: list[DiscoveryCandidate], failures: list[DiscoveryFailure], ) -> None: - route = o30_response_route(addressing.response_id) - messages = _collect( - transport, - (route,), - ( - build_o30_read( - MI_PRODUCT_INFO, - sub_index=0, - length=0x10, - request_id=addressing.request_id, + wire = TransportKind.CAN_FD if addressing.can_fd else TransportKind.CAN + route = hop_response_route(addressing.response_id) + messages: tuple[object, ...] = () + # Avoid burst replies overflowing deployed firmware on a busy classic bus. + for start in (0,) if addressing.can_fd else (0, 5, 10, 15): + messages += _collect( + transport, + (route,), + ( + build_hop_read( + MI_PRODUCT_INFO, + sub_index=start, + length=0x10 if addressing.can_fd else min(5, 0x10 - start), + request_id=addressing.request_id, + brs=addressing.brs, + can_fd=addressing.can_fd, + ), ), - ), - config, - ) + config, + ) matching: list[object] = [] + fragments: list[object] = [] malformed: Exception | None = None for message in messages: try: @@ -983,30 +1025,43 @@ def _probe_o30_endpoint( except HandError as error: malformed = error continue - if ( - frame is not None - and frame.main_index == MI_PRODUCT_INFO - and frame.sub_index == 0 - and not frame.rts - ): - matching.append(message) - if not matching and malformed is None: + if frame is not None and frame.main_index == MI_PRODUCT_INFO and not frame.rts: + fragments.append(message) + if frame.sub_index == 0: + matching.append(message) + if not fragments and malformed is None: return - spec = _transport_spec(source, channel, TransportKind.CAN_FD) + spec = _transport_spec(source, channel, wire) address = DiscoveryAddress( scheme="o30_hop_endpoint", request_id=addressing.request_id, response_id=route.arbitration_id, is_extended_id=False, ) + if fragments and not matching: + candidates.append( + _partial_candidate( + channel=channel, + wire_protocol=wire, + address=address, + transport=spec, + model=None, + side=None, + problem=DiscoveryProblem( + "product_code_incomplete", + "HOP product response is missing its first fragment", + ), + ) + ) + return if malformed is not None and not matching: candidates.append( _partial_candidate( channel=channel, - wire_protocol=TransportKind.CAN_FD, + wire_protocol=wire, address=address, transport=spec, - model="O30", + model=None, side=None, problem=_problem_from_error(malformed), ) @@ -1016,45 +1071,102 @@ def _probe_o30_endpoint( candidates.append( _partial_candidate( channel=channel, - wire_protocol=TransportKind.CAN_FD, + wire_protocol=wire, address=address, transport=spec, - model="O30", + model=None, side=None, problem=DiscoveryProblem( "address_collision", - "multiple O30 product responses used the same HOP endpoint", + "multiple product responses used the same HOP endpoint", ), ) ) return - provisional = get_declared_capabilities("O30", side=HandSide.LEFT) - transaction = o30_identity_transaction(provisional, addressing) + # The product object selects the profile; addresses never encode L6/O6 side. + model = None try: - progress = transaction.consume(matching[0]) - if not progress.matched or not progress.complete: - raise ProtocolError("O30 product-model probe response was incomplete") - next_transaction = transaction.build_next_transaction() - if next_transaction is None: - raise ProtocolError("O30 identity omitted required fields") + assembler = HOPRangeAssembler( + MI_PRODUCT_INFO, + sub_index=0, + length=0x10, + rts=False, + response_id=addressing.response_id, + ) + product_bytes = None + for message in fragments: + _, complete = assembler.consume(message) + if complete is not None: + product_bytes = complete + if product_bytes is None: + raise ProtocolError("incomplete HOP product response") + product = product_bytes.split(b"\x00", 1)[0].decode("utf-8").strip().casefold() + prefix = product.split("-")[0] + model = { + "l6": "L6", + "lhl6": "L6", + "o6": "O6", + "lho6": "O6", + "o30": "O30", + "o30i": "O30", + }.get(prefix) + except (UnicodeDecodeError, ProtocolError): + pass # The identity parser supplies the detailed malformed-product error. + if model is None: + candidates.append( + _partial_candidate( + channel=channel, + wire_protocol=wire, + address=replace(address, scheme="hop_endpoint"), + transport=spec, + model=None, + side=None, + problem=DiscoveryProblem( + "unsupported_product", "HOP product response has no supported model" + ), + ) + ) + return + if not addressing.can_fd and model not in {"O6", "O30"}: + return + if model != "O30": + address = replace(address, scheme="hop_endpoint") + provisional = get_declared_capabilities( + model, side=HandSide.LEFT, protocol=ProtocolKind.HOP, transport_kind=wire + ) + transaction = hop_identity_transaction(provisional, addressing) + try: + product_key = transaction.key + for message in fragments: + progress = transaction.consume(message) + if progress.complete: + next_transaction = transaction.build_next_transaction() + if next_transaction is None: + raise ProtocolError("HOP identity omitted required fields") + transaction = next_transaction + if transaction.key != product_key: + break + if transaction.key == product_key: + raise ProtocolError("HOP product-model probe response was incomplete") result = _execute_identity( transport, (route,), - next_transaction, + transaction, config, + **({"discard_s": 0} if not addressing.can_fd else {}), ) except _NoResponse: problem = DiscoveryProblem( "identity_timeout", - "O30 product model responded but complete identity timed out", + "HOP product model responded but complete identity timed out", ) candidates.append( _partial_candidate( channel=channel, - wire_protocol=TransportKind.CAN_FD, + wire_protocol=wire, address=address, transport=spec, - model="O30", + model=model, side=None, problem=problem, ) @@ -1063,10 +1175,10 @@ def _probe_o30_endpoint( candidates.append( _partial_candidate( channel=channel, - wire_protocol=TransportKind.CAN_FD, + wire_protocol=wire, address=address, transport=spec, - model="O30", + model=model, side=None, problem=_problem_from_error(wrapped.error), ) @@ -1076,22 +1188,22 @@ def _probe_o30_endpoint( raise _record_failure( failures, - operation="verify_o30_identity", + operation=f"verify_{model.lower()}_identity", error=wrapped.error, channel=channel, - wire_protocol=TransportKind.CAN_FD, + wire_protocol=wire, ) candidates.append( _partial_candidate( channel=channel, - wire_protocol=TransportKind.CAN_FD, + wire_protocol=wire, address=address, transport=spec, - model="O30", + model=model, side=None, problem=DiscoveryProblem( "transport_error", - "transport failed during complete O30 identity verification", + "transport failed during complete HOP identity verification", ), ) ) @@ -1099,10 +1211,10 @@ def _probe_o30_endpoint( candidates.append( _partial_candidate( channel=channel, - wire_protocol=TransportKind.CAN_FD, + wire_protocol=wire, address=address, transport=spec, - model="O30", + model=model, side=None, problem=_problem_from_error(error), ) @@ -1113,7 +1225,7 @@ def _probe_o30_endpoint( candidates.append( _verified_candidate( channel, - TransportKind.CAN_FD, + wire, address, spec, result, @@ -1124,7 +1236,7 @@ def _probe_o30_endpoint( candidates.append( DiscoveryCandidate( channel=channel, - wire_protocol=TransportKind.CAN_FD, + wire_protocol=wire, address=address, status=DiscoveryStatus.PARTIAL, identity=_identity_from_result(result, side=None), @@ -1132,7 +1244,7 @@ def _probe_o30_endpoint( problems=( DiscoveryProblem( "missing_side", - "O30 HOP identity does not provide a verified hand side", + "HOP identity does not provide a verified hand side", ), ), ) @@ -1339,7 +1451,9 @@ def _claim_results( route = _response_route_for_request(request, route_keys) discriminator = ( request.data[0] - if isinstance(request, can.Message) and request.data + if isinstance(request, can.Message) + and request.data + and route.arbitration_id == request.arbitration_id else None ) claims.setdefault((route, discriminator), result) @@ -1354,6 +1468,8 @@ def _response_route_for_request( candidates: list[CANRouteKey] = [] if isinstance(request, can.Message): candidates.append(CANRouteKey(request.arbitration_id, request.is_extended_id)) + if not request.is_extended_id: + candidates.append(CANRouteKey(request.arbitration_id | 0x400, False)) elif isinstance(request, CANFDMessage): if request.is_extended_id: # O20 responses use the same extended ID. L30 responses swap the @@ -1394,6 +1510,10 @@ def _response_after_claim( message.data[0] if isinstance(message, can.Message) and message.data else None ) claim = claims.get((route, discriminator)) + if claim is None: + # HOP classic uses request_id | 0x400 and may return an error MI instead + # of the requested MI. Its claim is endpoint-wide, as with HOP FD. + claim = claims.get((route, None)) # The broadcast C0 request is sent to 0xFF while a transport routes a # device's reply on its real 0x27/0x28 address. Permit that explicit # discovery claim for either default reply route only. diff --git a/src/linkerbot_hand/discovery/_naming.py b/src/linkerbot_hand/discovery/_naming.py index f4fa32b..446d3c9 100644 --- a/src/linkerbot_hand/discovery/_naming.py +++ b/src/linkerbot_hand/discovery/_naming.py @@ -90,7 +90,7 @@ def _endpoint_key(candidate: DiscoveryCandidate) -> _EndpointKey: candidate.channel.provider, candidate.channel.name, candidate.wire_protocol.value, - address.scheme, + "hop_endpoint" if address.scheme == "o30_hop_endpoint" else address.scheme, address.request_id, address.response_id, address.is_extended_id, diff --git a/src/linkerbot_hand/discovery/contracts.py b/src/linkerbot_hand/discovery/contracts.py index 5b96be3..6ba708a 100644 --- a/src/linkerbot_hand/discovery/contracts.py +++ b/src/linkerbot_hand/discovery/contracts.py @@ -11,7 +11,7 @@ from ..api.config import DeviceConfig from ..api.device_info import DeviceInfo, VersionField -from ..api.enums import HandSide, TransportKind +from ..api.enums import HandSide, ProtocolKind, TransportKind from ..api.errors import ValidationError from ..capabilities.types import DeviceCapabilities from ._naming import name_candidates @@ -53,6 +53,9 @@ class DiscoveryConfig: default_factory=MiniCANFDDiscoveryConfig ) + hop_request_ids: tuple[int, ...] | None = None + hop_brs: bool = False + def __post_init__(self) -> None: if self.backends is not None and ( not isinstance(self.backends, tuple) @@ -117,6 +120,19 @@ def __post_init__(self) -> None: raise ValidationError("l30_node_ids must contain node IDs within 1..31") if len(set(self.l30_node_ids)) != len(self.l30_node_ids): raise ValidationError("l30_node_ids must not contain duplicates") + if type(self.hop_brs) is not bool: + raise ValidationError("hop_brs must be bool") + if self.hop_request_ids is not None and ( + not isinstance(self.hop_request_ids, tuple) + or not self.hop_request_ids + or any( + type(v) is not int or not 0 <= v <= 0x3FE for v in self.hop_request_ids + ) + or len(set(self.hop_request_ids)) != len(self.hop_request_ids) + ): + raise ValidationError( + "hop_request_ids must contain unique IDs within 0x000..0x3FE" + ) if ( not isinstance(self.o30_request_ids, tuple) or not self.o30_request_ids @@ -362,7 +378,11 @@ class DiscoveryCandidate: device_info: DeviceInfo | None = None problems: tuple[DiscoveryProblem, ...] = () + hop_brs: bool = False + def __post_init__(self) -> None: + if type(self.hop_brs) is not bool: + raise ValidationError("hop_brs must be bool") if not isinstance(self.channel, DiscoveryChannel): raise ValidationError("channel must be DiscoveryChannel") if self.wire_protocol not in {TransportKind.CAN, TransportKind.CAN_FD}: @@ -478,29 +498,50 @@ def create_hand_device( identity_context=f"{self.channel.provider}:{self.channel.name} " f"({self.address})", ) - if model == "O30": - from ..protocols.o30.framing import HOPAddressing + if model == "O30" or self.address.scheme in { + "o30_hop_endpoint", + "hop_endpoint", + }: + from ..protocols.hop.framing import HOPAddressing - if self.address.scheme != "o30_hop_endpoint" or self.address.is_extended_id: - raise IncompleteIdentityError( - "O30 candidate has no standard HOP endpoint" - ) + if ( + self.address.scheme not in {"o30_hop_endpoint", "hop_endpoint"} + or self.address.is_extended_id + ): + raise IncompleteIdentityError("candidate has no standard HOP endpoint") addressing = HOPAddressing( self.address.request_id, self.address.response_id ) - if config is not None and config.o30_request_id not in ( - None, - addressing.request_id, - ): + if config is not None: + if config.hop_request_id not in (None, addressing.request_id): + raise ValidationError( + "hop_request_id disagrees with discovered endpoint" + ) + if config.o30_request_id not in (None, addressing.request_id): + raise ValidationError( + "o30_request_id disagrees with discovered endpoint" + ) + if model != "O30" and config.o30_request_id is not None: + raise ValidationError("o30_request_id is only valid for O30") + # Preserve the probe's BRS choice in the construction recipe. + brs = self.hop_brs + if config is not None and config.hop_transport is not self.wire_protocol: raise ValidationError( - "o30_request_id disagrees with discovered endpoint" + "hop_transport disagrees with discovered endpoint" ) + if config is not None and config.hop_brs != brs: + raise ValidationError("hop_brs disagrees with discovered endpoint") return HandDevice( model=model, side=resolved_side.value, + protocol=ProtocolKind.HOP, transport=self.transport.create_transport(), config=replace( - config or DeviceConfig(), o30_request_id=addressing.request_id + config or DeviceConfig(), + hop_request_id=addressing.request_id, + hop_brs=brs, + hop_transport=self.wire_protocol, + o30_request_id=addressing.request_id if model == "O30" else None, ), runtime_factory=runtime_factory, ) diff --git a/src/linkerbot_hand/protocols/base.py b/src/linkerbot_hand/protocols/base.py index 319cf3b..687bd61 100644 --- a/src/linkerbot_hand/protocols/base.py +++ b/src/linkerbot_hand/protocols/base.py @@ -7,6 +7,7 @@ from dataclasses import dataclass from typing import Protocol, runtime_checkable +from ..api.can_configuration import CANConfiguration from ..api.device_info import DeviceInfo from ..api.enums import DataOrigin from ..api.errors import ValidationError @@ -137,7 +138,7 @@ class ResponseProgress: matched: bool complete: bool = False - result: IdentityResult | SamplePayload | None = None + result: IdentityResult | SamplePayload | CANConfiguration | None = None publications: tuple[SamplePayload, ...] = () acknowledged: bool = False diff --git a/src/linkerbot_hand/protocols/hop/__init__.py b/src/linkerbot_hand/protocols/hop/__init__.py new file mode 100644 index 0000000..b7e7ecc --- /dev/null +++ b/src/linkerbot_hand/protocols/hop/__init__.py @@ -0,0 +1 @@ +"""Shared HOP framing, identity, sensors and model profiles.""" diff --git a/src/linkerbot_hand/protocols/hop/backend.py b/src/linkerbot_hand/protocols/hop/backend.py new file mode 100644 index 0000000..7094060 --- /dev/null +++ b/src/linkerbot_hand/protocols/hop/backend.py @@ -0,0 +1,118 @@ +"""Shared HOP endpoint state, identity, baseline and sensor handling.""" + +from __future__ import annotations + +from linkerbot_transport import CANRouteKey + +from ...api.enums import HandSide +from ...api.errors import ProtocolError, ValidationError +from ...api.readings import TactileSensorMetadata +from ...capabilities.types import ( + DeviceCapabilities, + TelemetrySemantic, +) +from ..base import IdentityResult, ProtocolTransaction, SamplePayload +from .framing import HOPAddressing, response_route +from .identity import identity_transaction +from .sensors import tactile_transaction + +_BASELINE = ( + TelemetrySemantic.ANGLES, + TelemetrySemantic.FAULTS, + TelemetrySemantic.STALL_DETECTION_TIMES, + TelemetrySemantic.STALL_THRESHOLDS, + TelemetrySemantic.STALL_HOLD_CURRENTS, +) + + +class HOPBackend: + """Model-independent HOP mechanics; subclasses define joint object layout.""" + + def __init__( + self, side: HandSide, *, request_id: int, brs: bool = False, can_fd: bool = True + ) -> None: + if side not in {HandSide.LEFT, HandSide.RIGHT}: + raise ProtocolError("HOP protocol requires a left or right side") + self._side = side + if type(request_id) is not int: + raise ValidationError("HOP request_id must be int") + self._addressing = HOPAddressing(request_id, request_id | 0x400, brs, can_fd) + # Retain the original O30 arbitration tag for compatibility; every + # HOP model uses this same physical address key. + self._endpoint_id = ( + "o30", + self._addressing.request_id, + self._addressing.response_id, + ) + self._route_keys = (response_route(self._addressing.response_id),) + self._sensor_metadata: dict[int, TactileSensorMetadata] = {} + self._identity_fingerprint: object | None = None + + @property + def endpoint_id(self) -> tuple[str, int, int]: + """Return the physical HOP endpoint used for TX arbitration.""" + + return self._endpoint_id + + @property + def route_keys(self) -> tuple[CANRouteKey, ...]: + """Return the exact standard response-ID route.""" + + return self._route_keys + + def identity_transaction( + self, + capabilities: DeviceCapabilities, + ) -> ProtocolTransaction: + """Build the response-gated field-wise identity read.""" + + if capabilities.side is not self._side: + raise ProtocolError("HOP capability side changed after backend creation") + # Initialization always supplies the unverified declared profile; + # same-generation get_device_info() supplies the verified profile. + if not capabilities.identity.verified: + self._sensor_metadata.clear() + self._identity_fingerprint = None + return identity_transaction( + capabilities, self._addressing, on_identity=self._observe_identity + ) + + def _observe_identity(self, identity: IdentityResult) -> None: + if identity.fingerprint != self._identity_fingerprint: + self._sensor_metadata.clear() + self._identity_fingerprint = identity.fingerprint + + def baseline_semantics( + self, + capabilities: DeviceCapabilities, + ) -> tuple[TelemetrySemantic, ...]: + """Return motion, fault, and safety configuration required before ready.""" + + for semantic in _BASELINE: + telemetry = capabilities.telemetry_channel(semantic) + if telemetry is None or not telemetry.device_queried: + raise ProtocolError( + f"HOP baseline semantic {semantic.value} is not queryable" + ) + return _BASELINE + + def sensor_transaction( + self, capabilities: DeviceCapabilities + ) -> ProtocolTransaction: + """Read descriptor-based force data with endpoint-scoped metadata.""" + return tactile_transaction( + capabilities, self._addressing, metadata_cache=self._sensor_metadata + ) + + def decode_report( + self, + message: object, + capabilities: DeviceCapabilities, + ) -> tuple[SamplePayload, ...]: + """Ignore unsolicited traffic; the profile uses explicit HOP queries.""" + + del message, capabilities + return () + + +__all__ = ["HOPBackend"] diff --git a/src/linkerbot_hand/protocols/hop/framing.py b/src/linkerbot_hand/protocols/hop/framing.py new file mode 100644 index 0000000..c9221b7 --- /dev/null +++ b/src/linkerbot_hand/protocols/hop/framing.py @@ -0,0 +1,242 @@ +"""Shared HOP framing for standard-ID classic CAN and CAN FD endpoints.""" + +from __future__ import annotations + +from dataclasses import dataclass + +import can + +from linkerbot_transport import CANRouteKey +from linkerbot_transport.canfd import CANFDMessage + +from ...api.enums import HandSide +from ...api.errors import ProtocolError, ValidationError + +# O30 defaults to a standard-frame CAN ID pair per side. The response +# ID is the request ID with the top standard-frame bit (0x400) set. +RIGHT_REQUEST_ID = 0x001 +RIGHT_RESPONSE_ID = 0x401 +LEFT_REQUEST_ID = 0x002 +LEFT_RESPONSE_ID = 0x402 +# Backwards-friendly aliases for the right-hand pair (used by discovery/tests). +REQUEST_ID = RIGHT_REQUEST_ID +RESPONSE_ID = RIGHT_RESPONSE_ID + +FRAME_TYPE = 0x04 +ERROR_MAIN_INDEX = 0x4F +RTS_MASK = 0x80 +MAIN_INDEX_MASK = 0x7F +MAX_FRAGMENT_LENGTH = 61 +_STANDARD_ID_MAX = 0x7FF + + +@dataclass(frozen=True, slots=True) +class HOPAddressing: + """One HOP endpoint's request/response standard-frame CAN ID pair.""" + + request_id: int + response_id: int + brs: bool = False + can_fd: bool = True + + def __post_init__(self) -> None: + if type(self.can_fd) is not bool: + raise ValidationError("HOP can_fd must be bool") + if not self.can_fd and self.brs: + raise ValidationError("classic HOP CAN cannot enable BRS") + if not isinstance(self.brs, bool): + raise ValidationError("HOP brs must be bool") + for value, name in ( + (self.request_id, "request_id"), + (self.response_id, "response_id"), + ): + if type(value) is not int or not 0 <= value <= _STANDARD_ID_MAX: + raise ValidationError(f"HOP {name} must be a standard CAN ID") + if self.request_id > 0x3FE: + raise ValidationError("HOP unicast request_id must be within 0x000..0x3FE") + if self.response_id != self.request_id | 0x400: + raise ValidationError("HOP response_id must be request_id | 0x400") + + +RIGHT_ADDRESSING = HOPAddressing(RIGHT_REQUEST_ID, RIGHT_RESPONSE_ID) +LEFT_ADDRESSING = HOPAddressing(LEFT_REQUEST_ID, LEFT_RESPONSE_ID) + + +def addressing_for_side(side: HandSide) -> HOPAddressing: + """Return the default CAN ID pair for ``side``; IDs may be configured.""" + + if side is HandSide.RIGHT: + return RIGHT_ADDRESSING + if side is HandSide.LEFT: + return LEFT_ADDRESSING + raise ValidationError("O30 addressing requires a left or right side") + + +@dataclass(frozen=True, slots=True) +class HOPFrame: + """One decoded HOP response with CAN FD padding removed.""" + + main_index: int + sub_index: int + payload: bytes + rts: bool + + +def response_route(response_id: int) -> CANRouteKey: + """Return the single standard-ID route consumed by an O30 endpoint.""" + + return CANRouteKey(response_id, False) + + +def build_read_message( + main_index: int, + *, + sub_index: int, + length: int, + request_id: int, + rts: bool = False, + brs: bool = False, + can_fd: bool = True, +) -> CANFDMessage | can.Message: + """Build one three-byte HOP object-range read.""" + + _validate_object_range(main_index, sub_index, length) + if not isinstance(brs, bool): + raise ValidationError("brs must be bool") + if not isinstance(rts, bool): + raise ValidationError("rts must be bool") + control = main_index | (RTS_MASK if rts else 0) + if type(can_fd) is not bool or (not can_fd and brs): + raise ValidationError("classic HOP CAN requires can_fd=False and BRS off") + if not can_fd: + return can.Message( + arbitration_id=request_id, + data=bytes((control, sub_index, length)), + is_extended_id=False, + check=True, + ) + return CANFDMessage( + arbitration_id=request_id, + data=bytes((control, sub_index, length)), + is_extended_id=False, + frame_type=FRAME_TYPE | (0x08 if brs else 0), + ) + + +def build_write_message( + main_index: int, + *, + sub_index: int, + payload: bytes, + request_id: int, + rts: bool = False, + brs: bool = False, + can_fd: bool = True, +) -> CANFDMessage | can.Message: + """Build one single-frame HOP object-range write.""" + + if not isinstance(payload, bytes) or not payload: + raise ValidationError("HOP write payload must be non-empty bytes") + if len(payload) > MAX_FRAGMENT_LENGTH: + raise ValidationError( + f"HOP write payload cannot exceed {MAX_FRAGMENT_LENGTH} bytes" + ) + if not isinstance(brs, bool): + raise ValidationError("brs must be bool") + if not isinstance(rts, bool): + raise ValidationError("rts must be bool") + _validate_object_range(main_index, sub_index, len(payload)) + control = main_index | (RTS_MASK if rts else 0) + if type(can_fd) is not bool or (not can_fd and brs): + raise ValidationError("classic HOP CAN requires can_fd=False and BRS off") + if not can_fd: + if len(payload) > 5: + raise ValidationError("classic HOP CAN write payload cannot exceed 5 bytes") + return can.Message( + arbitration_id=request_id, + data=bytes((control, sub_index, len(payload))) + payload, + is_extended_id=False, + check=True, + ) + return CANFDMessage( + arbitration_id=request_id, + data=bytes((control, sub_index, len(payload))) + payload, + is_extended_id=False, + frame_type=FRAME_TYPE | (0x08 if brs else 0), + ) + + +def matching_response_frame(message: object, response_id: int) -> HOPFrame | None: + """Decode one exact endpoint response or ignore unrelated CAN traffic.""" + + if ( + not isinstance(message, (CANFDMessage, can.Message)) + or message.is_extended_id + or message.arbitration_id != response_id + ): + return None + if isinstance(message, can.Message): + if message.is_error_frame or message.is_remote_frame: + return None + if message.is_fd or message.bitrate_switch or len(message.data) > 8: + return None + return decode_frame(bytes(message.data)) + + +def decode_frame(data: bytes) -> HOPFrame: + """Decode one response fragment using EDL instead of padded DLC length.""" + + if not isinstance(data, bytes) or len(data) < 3: + raise ProtocolError("HOP response is shorter than its header") + control, sub_index, effective_length = data[:3] + required = 3 + effective_length + if len(data) < required: + raise ProtocolError( + f"HOP response declares {effective_length} payload bytes but " + f"contains {max(0, len(data) - 3)}" + ) + return HOPFrame( + main_index=control & MAIN_INDEX_MASK, + sub_index=sub_index, + payload=data[3:required], + rts=bool(control & RTS_MASK), + ) + + +def _validate_object_range(main_index: int, sub_index: int, length: int) -> None: + for value, name, minimum, maximum in ( + (main_index, "main_index", 0, MAIN_INDEX_MASK), + (sub_index, "sub_index", 0, 0xFF), + (length, "length", 1, 0xFF), + ): + if type(value) is not int: + raise ValidationError(f"{name} must be int") + if value < minimum or value > maximum: + raise ValidationError(f"{name} must be within {minimum}..{maximum}") + if sub_index + length > 0x100: + raise ValidationError("sub_index + length cannot exceed 256") + + +__all__ = [ + "ERROR_MAIN_INDEX", + "FRAME_TYPE", + "LEFT_ADDRESSING", + "LEFT_REQUEST_ID", + "LEFT_RESPONSE_ID", + "MAIN_INDEX_MASK", + "MAX_FRAGMENT_LENGTH", + "REQUEST_ID", + "RESPONSE_ID", + "RIGHT_ADDRESSING", + "RIGHT_REQUEST_ID", + "RIGHT_RESPONSE_ID", + "RTS_MASK", + "HOPAddressing", + "HOPFrame", + "addressing_for_side", + "build_read_message", + "build_write_message", + "decode_frame", + "matching_response_frame", + "response_route", +] diff --git a/src/linkerbot_hand/protocols/hop/identity.py b/src/linkerbot_hand/protocols/hop/identity.py new file mode 100644 index 0000000..85bf827 --- /dev/null +++ b/src/linkerbot_hand/protocols/hop/identity.py @@ -0,0 +1,436 @@ +"""Shared HOP identity with model-specific layout and product validation.""" + +from __future__ import annotations + +import re +from collections.abc import Callable +from dataclasses import dataclass, replace + +from ...api.device_info import DeviceInfo, VersionField +from ...api.enums import HandSide +from ...api.errors import ( + IdentityMismatchError, + ProtocolError, + UnsupportedFirmwareError, +) +from ...api.readings import SampleMeta +from ...capabilities.types import DeviceCapabilities +from ..base import IdentityResult, ProtocolTransaction, ResponseProgress +from .framing import HOPAddressing, build_read_message +from .objects import MI_PRODUCT_INFO +from .response import HOPRangeAssembler, serialize_classic_read + +_EXPECTED_PROTOCOL_NAME = "HOP" +_VERSION_PATTERN = re.compile(r"^[VM]?\d+(?:\.\d+){1,2}$", re.IGNORECASE) +_DATE_PREFIX_PATTERN = re.compile(r"^\d{4}-\d{2}") + + +@dataclass(frozen=True, slots=True) +class _IdentityField: + name: str + sub_index: int + length: int + required: bool = False + + +_COMMON_FIELDS = ( + _IdentityField("product_model", 0x00, 0x10, required=True), + _IdentityField("power_supply_range", 0x10, 0x10), + _IdentityField("microcontroller_uid", 0x20, 0x19), + _IdentityField("device_uid", 0x39, 0x20, required=True), + _IdentityField("protocol_name", 0x59, 0x08, required=True), + _IdentityField("protocol_version", 0x61, 0x08, required=True), + _IdentityField("board_layout_probe", 0x69, 0x10, required=True), + _IdentityField("version_layout_probe", 0x99, 0x08), +) + +_DEPLOYED_FIELDS = ( + _IdentityField("control_board_version", 0x79, 0x08), + _IdentityField("bootloader_version", 0x81, 0x08), + _IdentityField("application_version", 0x89, 0x08, required=True), + _IdentityField("mechanical_version", 0x91, 0x08), + _IdentityField("build_time", 0x99, 0x20), + _IdentityField("supported_interfaces", 0xB9, 0x20), + _IdentityField("reported_side", 0xD9, 0x08), +) + +_DOCUMENTED_FIELDS = ( + _IdentityField("adapter_board_version", 0x79, 0x10), + _IdentityField("control_board_version", 0x89, 0x10), + _IdentityField("application_version", 0xA1, 0x08, required=True), + _IdentityField("mechanical_version", 0xA9, 0x08), + _IdentityField("build_time", 0xB1, 0x20), + _IdentityField("supported_interfaces", 0xD1, 0x20), + _IdentityField("reported_side", 0xF1, 0x08), +) + + +def identity_transaction( + capabilities: DeviceCapabilities, + addressing: HOPAddressing, + *, + on_identity: Callable[[IdentityResult], None] | None = None, +) -> ProtocolTransaction: + """Read identity without imposing a firmware-version connection gate.""" + + values: dict[str, bytes] = {} + return _field_transaction( + _COMMON_FIELDS, + 0, + capabilities, + values, + addressing, + next_factory=lambda: _layout_transaction( + capabilities, values, addressing, on_identity=on_identity + ), + ) + + +def _layout_transaction( + capabilities: DeviceCapabilities, + values: dict[str, bytes], + addressing: HOPAddressing, + *, + on_identity: Callable[[IdentityResult], None] | None, +) -> ProtocolTransaction: + layout = _select_layout(values) if capabilities.model == "O30" else "documented" + fields = _DEPLOYED_FIELDS if layout == "deployed" else _DOCUMENTED_FIELDS + + def result_factory() -> IdentityResult: + result = _identity_result(capabilities, values, layout=layout) + if on_identity is not None: + on_identity(result) + return result + + return _field_transaction( + fields, + 0, + capabilities, + values, + addressing, + result_factory=result_factory, + ) + + +def _field_transaction( + fields: tuple[_IdentityField, ...], + index: int, + capabilities: DeviceCapabilities, + values: dict[str, bytes], + addressing: HOPAddressing, + *, + next_factory: Callable[[], ProtocolTransaction] | None = None, + result_factory: Callable[[], IdentityResult] | None = None, +) -> ProtocolTransaction: + field = fields[index] + # In the compact layout, the probe is the first eight bytes of build_time. + # Reuse those bytes instead of asking the device twice. + prefix = ( + values["version_layout_probe"] + if field.name == "build_time" and field.sub_index == 0x99 + else b"" + ) + request_start = field.sub_index + len(prefix) + request_length = field.length - len(prefix) + assembler = HOPRangeAssembler( + MI_PRODUCT_INFO, + sub_index=request_start, + length=request_length, + rts=False, + response_id=addressing.response_id, + ) + final_field = index + 1 == len(fields) + + def build_next() -> ProtocolTransaction | None: + if not final_field: + return _field_transaction( + fields, + index + 1, + capabilities, + values, + addressing, + next_factory=next_factory, + result_factory=result_factory, + ) + if next_factory is not None: + return next_factory() + return None + + def consume(message: object) -> ResponseProgress: + matched, complete = assembler.consume(message) + if complete is None: + return ResponseProgress(matched=matched) + complete = prefix + complete + text = _decode_text(complete, field.name) + if field.required and text is None: + raise ProtocolError(f"HOP {field.name} is empty") + if text is not None: + _validate_identity_field(field.name, text, model=capabilities.model) + values[field.name] = complete + result = ( + result_factory() + if final_field and next_factory is None and result_factory is not None + else None + ) + return ResponseProgress(matched=True, complete=True, result=result) + + has_next = not final_field or next_factory is not None + transaction = ProtocolTransaction( + key=("identity", capabilities.model, field.name), + correlation_key=("hop", addressing.response_id), + request_messages=( + build_read_message( + MI_PRODUCT_INFO, + sub_index=request_start, + length=request_length, + request_id=addressing.request_id, + brs=addressing.brs, + can_fd=addressing.can_fd, + ), + ), + response_timeout_s=0.1, + estimated_duration_s=0.003, + response_consumer=consume, + late_response_guard_s=0.01, + next_transaction_factory=build_next if has_next else None, + ) + return serialize_classic_read(transaction, addressing) + + +def _select_layout(values: dict[str, bytes]) -> str: + board_probe = values["board_layout_probe"] + version_probe = _decode_text( + values["version_layout_probe"], + "version_layout_probe", + ) + # Splitting a documented UTF-8 board name at byte eight may cut a code + # point. Only the observed layout contains two NUL-padded ASCII versions. + second_board_bytes = board_probe[8:].split(b"\x00", 1)[0] + second_board_field = ( + second_board_bytes.decode("ascii") if second_board_bytes.isascii() else None + ) + if _looks_like_version(second_board_field) or ( + version_probe is not None + and _DATE_PREFIX_PATTERN.match(version_probe) is not None + ): + return "deployed" + return "documented" + + +def _identity_result( + declared: DeviceCapabilities, + values: dict[str, bytes], + *, + layout: str, +) -> IdentityResult: + product_model = _required_text(values, "product_model") + device_uid = _required_text(values, "device_uid") + protocol_name = _required_text(values, "protocol_name") + protocol_version = _required_text(values, "protocol_version") + board_probe = values["board_layout_probe"] + + if layout == "deployed": + interface_text = _decode_text( + board_probe[:8], + "interface_board_version", + ) + adapter_text = _decode_text( + board_probe[8:], + "adapter_board_version", + ) + interface_raw = board_probe[:8] + adapter_raw = board_probe[8:] + bootloader_raw = values["bootloader_version"] + else: + interface_text = _decode_text( + board_probe, + "interface_board_version", + ) + adapter_text = _decode_text( + values["adapter_board_version"], + "adapter_board_version", + ) + interface_raw = board_probe + adapter_raw = values["adapter_board_version"] + bootloader_raw = values["version_layout_probe"] + + control_text = _optional_text(values, "control_board_version") + bootloader_text = _decode_text(bootloader_raw, "bootloader_version") + application_text = _required_text(values, "application_version") + mechanical_text = _optional_text(values, "mechanical_version") + build_time = _optional_text(values, "build_time") + supported_interfaces = _optional_text(values, "supported_interfaces") + power_supply_range = _optional_text(values, "power_supply_range") + microcontroller_uid = _optional_text(values, "microcontroller_uid") + + interface = _version_or_none(interface_raw, interface_text) + adapter = _version_or_none(adapter_raw, adapter_text) + control = _version_or_none(values["control_board_version"], control_text) + bootloader = _version_or_none(bootloader_raw, bootloader_text) + firmware = _version(values["application_version"], application_text) + mechanical = _version_or_none(values["mechanical_version"], mechanical_text) + + side_field = _optional_text(values, "reported_side") + field_side = _parse_side(side_field) + product_side = _side_from_product_model(product_model) + if ( + field_side is not None + and product_side is not None + and field_side is not product_side + ): + raise IdentityMismatchError("HOP product model and reported side disagree") + reported_side = field_side or product_side + resolved_side = reported_side or declared.side + + verified_identity = replace( + declared.identity, + side=resolved_side, + verified=True, + firmware=firmware.text, + hardware_revision=None if interface is None else interface.text, + mechanical_revision=None if mechanical is None else mechanical.text, + sensor_variant=None, + serial_number=device_uid, + product_code=product_model, + protocol_name=protocol_name, + protocol_version=protocol_version, + power_supply_range=power_supply_range, + microcontroller_uid=microcontroller_uid, + adapter_board_version=None if adapter is None else adapter.text, + control_board_version=None if control is None else control.text, + bootloader_version=None if bootloader is None else bootloader.text, + build_time=build_time, + supported_interfaces=supported_interfaces, + reported_side=reported_side, + ) + verified = replace(declared, identity=verified_identity) + info = DeviceInfo( + model=verified.model, + serial_number=device_uid, + pcb_version=interface, + firmware_version=firmware, + mechanical_version=mechanical, + product_code=product_model, + sensor_variant=None, + side=resolved_side, + raw_identity=_raw_identity(values, layout=layout), + meta=SampleMeta.unavailable(), + adapter_board_version=adapter, + control_board_version=control, + bootloader_version=bootloader, + build_time=build_time, + supported_interfaces=supported_interfaces, + reported_side=reported_side, + protocol_name=protocol_name, + protocol_version=protocol_version, + power_supply_range=power_supply_range, + microcontroller_uid=microcontroller_uid, + ) + fingerprint = ( + product_model, + device_uid, + protocol_name, + protocol_version, + None if interface is None else interface.text, + None if adapter is None else adapter.text, + None if control is None else control.text, + None if bootloader is None else bootloader.text, + firmware.text, + None if mechanical is None else mechanical.text, + None if reported_side is None else reported_side.value, + ) + return IdentityResult( + capabilities=verified, + device_info=info, + fingerprint=fingerprint, + ) + + +def _raw_identity(values: dict[str, bytes], *, layout: str) -> bytes: + fields = _COMMON_FIELDS + ( + _DEPLOYED_FIELDS if layout == "deployed" else _DOCUMENTED_FIELDS + ) + length = 0xE1 if layout == "deployed" else 0xF9 + raw = bytearray(length) + for field in fields: + data = values[field.name] + raw[field.sub_index : field.sub_index + field.length] = data + return bytes(raw) + + +def _validate_identity_field(name: str, value: str, *, model: str) -> None: + normalized = value.casefold() + prefixes = {"O30": ("o30", "o30i"), "L6": ("l6", "lhl6"), "O6": ("o6", "lho6")}[ + model + ] + if name == "product_model" and not any( + normalized == prefix or normalized.startswith(prefix + "-") + for prefix in prefixes + ): + raise IdentityMismatchError(f"expected {model} product model, got {value!r}") + if name == "protocol_name" and value.upper() != _EXPECTED_PROTOCOL_NAME: + raise UnsupportedFirmwareError(f"HOP protocol {value!r} is unsupported") + + +def _required_text(values: dict[str, bytes], name: str) -> str: + value = _optional_text(values, name) + if value is None: + raise ProtocolError(f"HOP {name} is empty") + return value + + +def _optional_text(values: dict[str, bytes], name: str) -> str | None: + return _decode_text(values[name], name) + + +def _decode_text(data: bytes, name: str) -> str | None: + try: + value = data.split(b"\x00", 1)[0].decode("utf-8").strip() + except UnicodeDecodeError as error: + raise ProtocolError(f"HOP {name} is not UTF-8") from error + return value or None + + +def _looks_like_version(value: str | None) -> bool: + return value is not None and _VERSION_PATTERN.fullmatch(value) is not None + + +def _parse_side(value: str | None) -> HandSide | None: + if value is None: + return None + normalized = value.strip().casefold() + if normalized in {"l", "left"}: + return HandSide.LEFT + if normalized in {"r", "right"}: + return HandSide.RIGHT + raise ProtocolError(f"HOP reported side {value!r} is invalid") + + +def _side_from_product_model(value: str) -> HandSide | None: + parts = value.split("-") + if len(parts) < 4: + return None + return {"L": HandSide.LEFT, "R": HandSide.RIGHT}.get(parts[3].upper()) + + +def _version(raw: bytes, text: str) -> VersionField: + semantic = text.removeprefix("V").removeprefix("v") + semantic = semantic.removeprefix("M").removeprefix("m") + parts = semantic.split(".") + if len(parts) == 3 and all(part.isdigit() for part in parts): + major, minor, patch = (int(part) for part in parts) + return VersionField( + raw=raw, + text=text, + major=major, + minor=minor, + patch=patch, + ) + return VersionField(raw=raw, text=text) + + +def _version_or_none(raw: bytes, text: str | None) -> VersionField | None: + return None if text is None else _version(raw, text) + + +__all__ = ["identity_transaction"] diff --git a/src/linkerbot_hand/protocols/hop/management.py b/src/linkerbot_hand/protocols/hop/management.py new file mode 100644 index 0000000..87130ce --- /dev/null +++ b/src/linkerbot_hand/protocols/hop/management.py @@ -0,0 +1,149 @@ +"""HOP 0x35 configuration reads and single-frame reconfiguration writes.""" + +from ...api.can_configuration import DATA_BITRATES, NOMINAL_BITRATES, CANConfiguration +from ...api.errors import ProtocolError +from ..base import ProtocolTransaction, ResponseProgress +from .framing import HOPAddressing, build_read_message, build_write_message +from .response import HOPRangeAssembler + + +def configuration_transaction(addressing: HOPAddressing) -> ProtocolTransaction: + values: dict[int, bytes] = {} + spans = ((0, 2), (6, 4)) + + def stage(index: int) -> ProtocolTransaction: + start, count = spans[index] + assembler = HOPRangeAssembler( + 0x35, + sub_index=start, + length=count, + rts=True, + response_id=addressing.response_id, + ) + + def consume(message: object) -> ResponseProgress: + matched, complete = assembler.consume(message) + if complete is None: + return ResponseProgress(matched=matched) + values[start] = complete + result = None + if index == 1: + interface, brs, nominal, data = complete + if ( + interface not in (1, 2) + or brs not in (0, 1) + or not 1 <= nominal <= len(NOMINAL_BITRATES) + or not 1 <= data <= len(DATA_BITRATES) + ): + raise ProtocolError("HOP returned invalid CAN configuration codes") + request_id = int.from_bytes(values[0], "little") + if not 0 <= request_id <= 0x3FE: + raise ProtocolError("HOP returned invalid standard CAN ID") + result = CANConfiguration( + request_id, + interface, + bool(brs), + NOMINAL_BITRATES[nominal - 1], + DATA_BITRATES[data - 1], + ) + return ResponseProgress(matched=True, complete=True, result=result) + + return ProtocolTransaction( + key=("hop_can_configuration", index), + correlation_key=("hop", addressing.response_id), + request_messages=( + build_read_message( + 0x35, + sub_index=start, + length=count, + rts=True, + request_id=addressing.request_id, + brs=addressing.brs, + can_fd=addressing.can_fd, + ), + ), + response_timeout_s=0.1, + estimated_duration_s=0.003, + response_consumer=consume, + late_response_guard_s=0.01, + next_transaction_factory=(lambda: stage(1)) if index == 0 else None, + ) + + return stage(0) + + +def configuration_write( + addressing: HOPAddressing, sub_index: int, payload: bytes +) -> ProtocolTransaction: + """Dispatch only: applying a new route can prevent an old-route ACK.""" + valid = ( + ( + sub_index == 0 + and len(payload) == 2 + and int.from_bytes(payload, "little") <= 0x3FE + ) + or (sub_index == 7 and payload in (b"\x00", b"\x01")) + or ( + sub_index == 8 + and len(payload) in (1, 2) + and 1 <= payload[0] <= len(NOMINAL_BITRATES) + and (len(payload) == 1 or 1 <= payload[1] <= len(DATA_BITRATES)) + ) + or ( + sub_index == 9 + and len(payload) == 1 + and 1 <= payload[0] <= len(DATA_BITRATES) + ) + ) + if not valid: + raise ProtocolError("invalid HOP CAN configuration write") + return ProtocolTransaction( + key=("hop_can_write", sub_index), + correlation_key=("hop", addressing.response_id), + request_messages=( + build_write_message( + 0x35, + sub_index=sub_index, + payload=payload, + rts=True, + request_id=addressing.request_id, + brs=addressing.brs, + can_fd=addressing.can_fd, + ), + ), + response_timeout_s=0, + estimated_duration_s=0.003, + ) + + +def save_transaction(addressing: HOPAddressing) -> ProtocolTransaction: + assembler = HOPRangeAssembler( + 0x35, sub_index=0x1E, length=1, rts=True, response_id=addressing.response_id + ) + + def consume(message: object) -> ResponseProgress: + matched, data = assembler.consume(message) + return ResponseProgress( + matched=matched, complete=data is not None, acknowledged=data is not None + ) + + return ProtocolTransaction( + key=("hop_save",), + correlation_key=("hop", addressing.response_id), + request_messages=( + build_write_message( + 0x35, + sub_index=0x1E, + payload=b"\x01", + rts=True, + request_id=addressing.request_id, + brs=addressing.brs, + can_fd=addressing.can_fd, + ), + ), + response_timeout_s=0.2, + estimated_duration_s=0.005, + response_consumer=consume, + response_acknowledges_command=True, + late_response_guard_s=0.01, + ) diff --git a/src/linkerbot_hand/protocols/hop/objects.py b/src/linkerbot_hand/protocols/hop/objects.py new file mode 100644 index 0000000..65a9730 --- /dev/null +++ b/src/linkerbot_hand/protocols/hop/objects.py @@ -0,0 +1,30 @@ +"""Standard HOP object indices shared by supported model layouts.""" + +from ...api.errors import ProtocolError +from ...api.readings import JointFaultStatus + +MI_PRODUCT_INFO = 0x41 + +MI_SENSOR_INFO = 0x31 +MI_SENSOR_DATA_1 = 0x32 +MI_SENSOR_DATA_2 = 0x33 +MI_SENSOR_DATA_3 = 0x34 + + +def decode_fault_statuses(raw: tuple[int, ...]) -> tuple[JointFaultStatus, ...]: + """Decode shared HOP fault bits, keeping the original byte and reserved bits.""" + if any(type(value) is not int or not 0 <= value <= 255 for value in raw): + raise ProtocolError("HOP faults require unsigned bytes") + return tuple( + JointFaultStatus( + raw=v, + hand_layer_stall=bool(v & 1), + actuator_layer_stall=bool(v & 2), + over_current=bool(v & 4), + over_temperature=bool(v & 8), + actuator_abnormal=bool(v & 64), + actuator_offline=bool(v & 128), + reserved_bits=v & 0x30, + ) + for v in raw + ) diff --git a/src/linkerbot_hand/protocols/hop/response.py b/src/linkerbot_hand/protocols/hop/response.py new file mode 100644 index 0000000..ad9c3a1 --- /dev/null +++ b/src/linkerbot_hand/protocols/hop/response.py @@ -0,0 +1,186 @@ +"""Range-aware HOP response assembly and device-error classification.""" + +from __future__ import annotations + +from dataclasses import replace + +import can + +from ...api.errors import ProtocolError, UnsupportedFirmwareError +from ..base import ProtocolTransaction, ResponseProgress +from .framing import ( + ERROR_MAIN_INDEX, + HOPAddressing, + build_read_message, + matching_response_frame, +) + +_DEVICE_ERROR_MESSAGES = { + 0x01: "main index does not exist", + 0x02: "sub-index does not exist", + 0x04: "register is not writable", + 0x08: "data length mismatch", + 0x10: "permission denied", + 0x20: "returned data contains zero padding", +} + + +class HOPRangeAssembler: + """Assemble one serialized HOP object range by absolute sub-index.""" + + def __init__( + self, + main_index: int, + *, + sub_index: int, + length: int, + rts: bool, + response_id: int, + object_unavailable_is_unsupported: bool = False, + ) -> None: + self._main_index = main_index + self._sub_index = sub_index + self._length = length + self._rts = rts + self._response_id = response_id + self._object_unavailable_is_unsupported = object_unavailable_is_unsupported + self._buffer = bytearray(length) + self._received = bytearray(length) + + def consume(self, message: object) -> tuple[bool, bytes | None]: + """Return ``(matched, complete_payload)`` for one routed message.""" + + frame = matching_response_frame(message, self._response_id) + if frame is None: + return False, None + if ( + frame.main_index == ERROR_MAIN_INDEX + and self._main_index != ERROR_MAIN_INDEX + ): + if len(frame.payload) != 1: + raise ProtocolError("HOP error response must contain one byte") + code = frame.payload[0] + if code == 0: + return False, None + descriptions = [ + message for bit, message in _DEVICE_ERROR_MESSAGES.items() if code & bit + ] + if code & ~0x3F: + descriptions.append(f"unknown flags 0x{code & ~0x3F:02X}") + description = "; ".join(descriptions) + if ( + self._object_unavailable_is_unsupported + and code & 0x03 + and not code & ~0x23 + ): + raise UnsupportedFirmwareError( + f"HOP object 0x{self._main_index:02X}/" + f"0x{self._sub_index:02X} is unavailable " + f"(error 0x{code:02X}): {description}" + ) + # 0x4F reports communication flags, not joint/mechanical faults. + # Its SI is an error-history slot, not the rejected object's SI. + raise ProtocolError( + f"HOP communication error 0x{code:02X} at history slot " + f"0x{frame.sub_index:02X}: {description}" + ) + if frame.main_index != self._main_index or frame.rts != self._rts: + return False, None + if not frame.payload: + raise ProtocolError("HOP response fragment is empty") + start = frame.sub_index - self._sub_index + end = start + len(frame.payload) + if start < 0 or end > self._length: + if end > 0 and start < self._length: + raise ProtocolError( + "HOP response fragment overlaps requested range but exceeds it" + ) + return False, None + for offset, value in enumerate(frame.payload, start=start): + if self._received[offset] and self._buffer[offset] != value: + raise ProtocolError("HOP duplicate fragment conflicts") + self._buffer[offset] = value + self._received[offset] = 1 + if not all(self._received): + return True, None + return True, bytes(self._buffer) + + +def serialize_classic_read( + transaction: ProtocolTransaction, addressing: HOPAddressing +) -> ProtocolTransaction: + """Gate each five-byte classic read on its reply, reusing the full decoder. + + Busy shared buses can expose firmware that drops burst response fragments. + Keep the original consumer across stages so only a complete object produces + a result. FD requests retain their existing framing and transaction shape. + """ + if addressing.can_fd: + return transaction + message = transaction.request_messages[0] + if not isinstance(message, can.Message) or len(message.data) != 3: + raise ProtocolError("serialized classic read requires one HOP read request") + control, start, length = bytes(message.data) + if length <= 5: + return transaction + count = (length + 4) // 5 + + def stage(offset: int) -> ProtocolTransaction: + size = min(5, length - offset) + final = offset + size == length + assembler = HOPRangeAssembler( + control & 0x7F, + sub_index=start + offset, + length=size, + rts=bool(control & 0x80), + response_id=addressing.response_id, + ) + + def consume(response: object) -> ResponseProgress: + # Preserve the original object's unsupported/error classification. + frame = matching_response_frame(response, addressing.response_id) + if frame is not None and frame.main_index == ERROR_MAIN_INDEX: + return transaction.consume(response) + matched, complete = assembler.consume(response) + if not matched: + return ResponseProgress(matched=False) + progress = transaction.consume(response) + if complete is None: + return ResponseProgress(matched=matched) + if final: + if not progress.complete: + raise ProtocolError("serialized HOP read omitted object bytes") + return progress + return ResponseProgress(matched=True, complete=True) + + next_factory = None + if not final: + next_factory = lambda: stage(offset + size) + elif ( + transaction.next_transaction is not None + or transaction.next_transaction_factory is not None + ): + next_factory = transaction.build_next_transaction + return replace( + transaction, + request_messages=( + build_read_message( + control & 0x7F, + sub_index=start + offset, + length=size, + request_id=addressing.request_id, + rts=bool(control & 0x80), + can_fd=False, + ), + ), + estimated_duration_s=transaction.estimated_duration_s / count, + response_consumer=consume, + next_transaction=None, + next_transaction_factory=next_factory, + yield_after_response=transaction.yield_after_response if final else False, + ) + + return stage(0) + + +__all__ = ["HOPRangeAssembler", "serialize_classic_read"] diff --git a/src/linkerbot_hand/protocols/hop/sensors.py b/src/linkerbot_hand/protocols/hop/sensors.py new file mode 100644 index 0000000..578d6b5 --- /dev/null +++ b/src/linkerbot_hand/protocols/hop/sensors.py @@ -0,0 +1,814 @@ +"""Descriptor-driven HOP HOP 0.0.4 tactile sensor transactions.""" + +from __future__ import annotations + +import math +import struct +from collections.abc import Callable +from dataclasses import replace + +from ...api.enums import DataOrigin +from ...api.errors import ( + ProtocolError, + UnsupportedFirmwareError, + UnsupportedOperationError, +) +from ...api.readings import ( + TactileFieldDescriptor, + TactileFieldSample, + TactileSensorMetadata, + TactileSensorSample, +) +from ...capabilities.types import DeviceCapabilities, TelemetrySemantic +from ..base import ProtocolTransaction, ResponseProgress, SamplePayload +from .framing import ( + MAX_FRAGMENT_LENGTH, + HOPAddressing, + build_read_message, + build_write_message, + matching_response_frame, +) +from .objects import ( + MI_SENSOR_DATA_1, + MI_SENSOR_DATA_2, + MI_SENSOR_DATA_3, + MI_SENSOR_INFO, +) +from .response import HOPRangeAssembler, serialize_classic_read + +SENSOR_NAMES = ("thumb", "index", "middle", "ring", "pinky") + +_SI_SENSOR_TYPE = 0x00 +_SI_DATA_UNIT = 0x20 +_SI_DATA_RANGE = 0x28 +_SI_MAXIMUM_ROWS = 0x29 +_SI_MAXIMUM_COLUMNS = 0x2A +_SI_DESCRIPTOR_LENGTH = 0x2B +_SI_DESCRIPTOR = 0x2C +_SI_TOTAL_DATA_LENGTH = 0x6C +_SI_SELECTED_SENSOR = 0x6E +_SI_REQUESTED_ROWS = 0x6F +_SI_REQUESTED_COLUMNS = 0x70 +_SI_VALID_DATA_POSITION_MODE = 0x71 +_SI_RECTANGLE_FILL = 0x72 + +_METADATA_PREFIX_LENGTH = _SI_DESCRIPTOR - _SI_SENSOR_TYPE +_MAX_DESCRIPTOR_LENGTH = _SI_TOTAL_DATA_LENGTH - _SI_DESCRIPTOR +_SENSOR_CONTROL_LENGTH = 5 +_MAX_SENSOR_DATA_LENGTH = 765 +_STAGE_ESTIMATE_S = 0.003 +_RESPONSE_TIMEOUT_S = 0.1 +_LATE_RESPONSE_GUARD_S = 0.01 + +_SEMANTICS = frozenset( + {"Fn", "Ft", "Fd", "Px", "FnS", "FtS", "FdS", "Rsv", "x", "y", "z", "xyz"} +) +_DATA_TYPES = { + "U8": ("B", 1), + "I8": ("b", 1), + "U16": ("H", 2), + "I16": ("h", 2), + "U32": ("I", 4), + "I32": ("i", 4), + "F32": ("f", 4), +} + +_NextFactory = Callable[[], ProtocolTransaction | None] + + +def parse_tactile_descriptor( + descriptor_text: str, + *, + total_data_length: int | None = None, +) -> tuple[TactileFieldDescriptor, ...]: + """Parse one 0.0.4 descriptor and calculate every byte range.""" + + if not isinstance(descriptor_text, str) or not descriptor_text: + raise ProtocolError("HOP tactile descriptor must be a non-empty string") + if total_data_length is not None and ( + type(total_data_length) is not int or total_data_length < 0 + ): + raise ProtocolError("HOP tactile total data length must be non-negative") + + fields: list[TactileFieldDescriptor] = [] + byte_offset = 0 + for item in descriptor_text.split(";"): + parts = item.split("_") + if len(parts) != 3: + raise ProtocolError(f"HOP tactile descriptor field {item!r} is malformed") + semantic, data_type, count_text = parts + semantic = {"FNS": "FnS", "FTS": "FtS", "FDS": "FdS"}.get(semantic, semantic) + if semantic not in _SEMANTICS: + raise ProtocolError( + f"HOP tactile descriptor semantic {semantic!r} is unsupported" + ) + try: + _format, scalar_size = _DATA_TYPES[data_type] + except KeyError as error: + raise ProtocolError( + f"HOP tactile descriptor data type {data_type!r} is unsupported" + ) from error + if ( + not count_text.isascii() + or not count_text.isdecimal() + or count_text.startswith("0") + ): + raise ProtocolError( + f"HOP tactile descriptor count {count_text!r} is invalid" + ) + element_count = int(count_text) + scalar_count = element_count * (3 if semantic == "xyz" else 1) + byte_length = scalar_count * scalar_size + fields.append( + TactileFieldDescriptor( + semantic=semantic, + data_type=data_type, + element_count=element_count, + byte_offset=byte_offset, + byte_length=byte_length, + ) + ) + byte_offset += byte_length + + if total_data_length is not None and byte_offset != total_data_length: + raise ProtocolError( + "HOP tactile descriptor defines " + f"{byte_offset} bytes but device reports {total_data_length}" + ) + return tuple(fields) + + +def decode_tactile_fields( + descriptors: tuple[TactileFieldDescriptor, ...], + raw_data: bytes, +) -> tuple[TactileFieldSample, ...]: + """Decode exact little-endian values while preserving the source bytes.""" + + if not isinstance(descriptors, tuple) or not descriptors: + raise ProtocolError("HOP tactile field descriptors are empty") + if any(not isinstance(value, TactileFieldDescriptor) for value in descriptors): + raise ProtocolError("HOP tactile descriptors contain an invalid field") + if not isinstance(raw_data, bytes): + raise ProtocolError("HOP tactile data must be bytes") + expected_length = sum(value.byte_length for value in descriptors) + if len(raw_data) != expected_length: + raise ProtocolError( + f"HOP tactile data contains {len(raw_data)} bytes; " + f"descriptor requires {expected_length}" + ) + + samples: list[TactileFieldSample] = [] + for descriptor in descriptors: + format_code, _scalar_size = _DATA_TYPES[descriptor.data_type] + scalar_count = descriptor.element_count * ( + 3 if descriptor.semantic == "xyz" else 1 + ) + values = struct.unpack_from( + f"<{scalar_count}{format_code}", + raw_data, + descriptor.byte_offset, + ) + if any( + isinstance(value, float) and not math.isfinite(value) for value in values + ): + raise ProtocolError("HOP tactile F32 data must contain finite values") + decoded_values = ( + tuple( + (values[index], values[index + 1], values[index + 2]) + for index in range(0, len(values), 3) + ) + if descriptor.semantic == "xyz" + else tuple(values) + ) + samples.append( + TactileFieldSample( + semantic=descriptor.semantic, + data_type=descriptor.data_type, + element_count=descriptor.element_count, + byte_offset=descriptor.byte_offset, + byte_length=descriptor.byte_length, + values=decoded_values, + ) + ) + return tuple(samples) + + +def sensor_data_chunks(total_data_length: int) -> tuple[tuple[int, int, int], ...]: + """Return ``(main_index, sub_index, length)`` chunks for global sensor bytes.""" + + if type(total_data_length) is not int or not ( + 1 <= total_data_length <= _MAX_SENSOR_DATA_LENGTH + ): + raise ProtocolError( + f"HOP tactile data length must be in 1..{_MAX_SENSOR_DATA_LENGTH}" + ) + chunks: list[tuple[int, int, int]] = [] + global_offset = 0 + while global_offset < total_data_length: + if global_offset < 255: + main_index, channel_start, channel_end = MI_SENSOR_DATA_1, 0, 255 + elif global_offset < 510: + main_index, channel_start, channel_end = MI_SENSOR_DATA_2, 255, 510 + else: + main_index, channel_start, channel_end = MI_SENSOR_DATA_3, 510, 765 + length = min( + MAX_FRAGMENT_LENGTH, + total_data_length - global_offset, + channel_end - global_offset, + ) + chunks.append((main_index, global_offset - channel_start, length)) + global_offset += length + return tuple(chunks) + + +def tactile_transaction( + capabilities: DeviceCapabilities, + addressing: HOPAddressing, + *, + metadata_cache: dict[int, TactileSensorMetadata], +) -> ProtocolTransaction: + """Read all five HOP fingertip sensors as one atomic force sample.""" + + telemetry = capabilities.telemetry_channel(TelemetrySemantic.FORCE) + if telemetry is None or not telemetry.device_queried: + raise UnsupportedOperationError("HOP has no active tactile query") + if set(metadata_cache) == set(range(1, len(SENSOR_NAMES) + 1)): + return replace( + _data_transaction(metadata_cache, addressing=addressing), + firmware_optional=True, + ) + + if not addressing.can_fd: + # Some deployed O30 firmware reports NO_SENSOR but does not implement + # the selector ACK. A single absent finger must not suppress the others. + no_sensor = False + + def check_sensor_type(data: bytes) -> None: + nonlocal no_sensor + no_sensor = data.split(b"\x00", 1)[0].strip() == b"NO_SENSOR" + + def select_metadata() -> ProtocolTransaction: + transaction = _metadata_select_transaction( + 1, {}, addressing=addressing, metadata_cache=metadata_cache + ) + if not no_sensor or capabilities.identity.firmware != "1.0.0": + return transaction + type_reply = HOPRangeAssembler( + MI_SENSOR_INFO, + sub_index=_SI_SENSOR_TYPE, + length=32, + rts=False, + response_id=addressing.response_id, + ) + + def consume(message: object) -> ResponseProgress: + frame = matching_response_frame(message, addressing.response_id) + if frame is not None and frame.main_index == MI_SENSOR_INFO: + # Captured O30 1.0.0 repeats the complete NO_SENSOR type + # instead of selector ACK. Faults and other object replies + # cannot establish absence and go through normal matching. + matched, complete = type_reply.consume(message) + if complete is not None: + if complete.split(b"\x00", 1)[0].strip() == b"NO_SENSOR": + raise UnsupportedFirmwareError( + "O30 repeats NO_SENSOR instead of tactile selector ACK" + ) + raise ProtocolError("unexpected sensor type during selection") + if matched: + return ResponseProgress(matched=True) + return transaction.consume(message) + + return replace(transaction, response_consumer=consume) + + return replace( + _read_stage( + key=("read", TelemetrySemantic.FORCE, "sensor_presence"), + main_index=MI_SENSOR_INFO, + sub_index=_SI_SENSOR_TYPE, + length=32, + on_data=check_sensor_type, + addressing=addressing, + next_factory=select_metadata, + ), + firmware_optional=True, + ) + + metadata: dict[int, TactileSensorMetadata] = {} + first = _metadata_select_transaction( + 1, + metadata, + addressing=addressing, + metadata_cache=metadata_cache, + ) + return replace(first, firmware_optional=True) + + +def _metadata_select_transaction( + selected_sensor: int, + metadata: dict[int, TactileSensorMetadata], + *, + addressing: HOPAddressing, + metadata_cache: dict[int, TactileSensorMetadata], +) -> ProtocolTransaction: + payload = bytes((selected_sensor,)) + + def next_stage() -> ProtocolTransaction: + return _metadata_prefix_transaction( + selected_sensor, + metadata, + addressing=addressing, + metadata_cache=metadata_cache, + ) + + return _write_stage( + key=("read", TelemetrySemantic.FORCE, "select_metadata", selected_sensor), + sub_index=_SI_SELECTED_SENSOR, + payload=payload, + addressing=addressing, + next_factory=next_stage, + ) + + +def _metadata_prefix_transaction( + selected_sensor: int, + metadata: dict[int, TactileSensorMetadata], + *, + addressing: HOPAddressing, + metadata_cache: dict[int, TactileSensorMetadata], +) -> ProtocolTransaction: + values: dict[str, bytes] = {} + + def consume_prefix(data: bytes) -> None: + values["prefix"] = data + descriptor_length = data[_SI_DESCRIPTOR_LENGTH] + if descriptor_length > _MAX_DESCRIPTOR_LENGTH: + raise ProtocolError( + f"HOP tactile descriptor length {descriptor_length} exceeds 64" + ) + + def next_stage() -> ProtocolTransaction: + descriptor_length = values["prefix"][_SI_DESCRIPTOR_LENGTH] + if descriptor_length == 0: + values["descriptor"] = b"" + return _metadata_total_length_transaction( + selected_sensor, + metadata, + values, + addressing=addressing, + metadata_cache=metadata_cache, + ) + return _metadata_descriptor_transaction( + selected_sensor, + metadata, + values, + descriptor_length=descriptor_length, + addressing=addressing, + metadata_cache=metadata_cache, + ) + + return _read_stage( + key=("read", TelemetrySemantic.FORCE, "metadata_prefix", selected_sensor), + main_index=MI_SENSOR_INFO, + sub_index=_SI_SENSOR_TYPE, + length=_METADATA_PREFIX_LENGTH, + on_data=consume_prefix, + addressing=addressing, + next_factory=next_stage, + ) + + +def _metadata_descriptor_transaction( + selected_sensor: int, + metadata: dict[int, TactileSensorMetadata], + values: dict[str, bytes], + *, + descriptor_length: int, + addressing: HOPAddressing, + metadata_cache: dict[int, TactileSensorMetadata], +) -> ProtocolTransaction: + def consume_descriptor(data: bytes) -> None: + values["descriptor"] = data + + def next_stage() -> ProtocolTransaction: + return _metadata_total_length_transaction( + selected_sensor, + metadata, + values, + addressing=addressing, + metadata_cache=metadata_cache, + ) + + return _read_stage( + key=("read", TelemetrySemantic.FORCE, "descriptor", selected_sensor), + main_index=MI_SENSOR_INFO, + sub_index=_SI_DESCRIPTOR, + length=descriptor_length, + on_data=consume_descriptor, + addressing=addressing, + next_factory=next_stage, + ) + + +def _metadata_total_length_transaction( + selected_sensor: int, + metadata: dict[int, TactileSensorMetadata], + values: dict[str, bytes], + *, + addressing: HOPAddressing, + metadata_cache: dict[int, TactileSensorMetadata], +) -> ProtocolTransaction: + def consume_total_length(data: bytes) -> None: + values["total_data_length"] = data + + def next_stage() -> ProtocolTransaction: + return _metadata_controls_transaction( + selected_sensor, + metadata, + values, + addressing=addressing, + metadata_cache=metadata_cache, + ) + + return _read_stage( + key=("read", TelemetrySemantic.FORCE, "total_length", selected_sensor), + main_index=MI_SENSOR_INFO, + sub_index=_SI_TOTAL_DATA_LENGTH, + length=2, + on_data=consume_total_length, + addressing=addressing, + next_factory=next_stage, + ) + + +def _metadata_controls_transaction( + selected_sensor: int, + metadata: dict[int, TactileSensorMetadata], + values: dict[str, bytes], + *, + addressing: HOPAddressing, + metadata_cache: dict[int, TactileSensorMetadata], +) -> ProtocolTransaction: + final_sensor = selected_sensor == len(SENSOR_NAMES) + + def consume_controls(data: bytes) -> None: + parsed = _build_metadata(selected_sensor, values, data) + metadata[selected_sensor] = parsed + if final_sensor: + metadata_cache.clear() + metadata_cache.update(metadata) + + def next_stage() -> ProtocolTransaction: + if not final_sensor: + return _metadata_select_transaction( + selected_sensor + 1, + metadata, + addressing=addressing, + metadata_cache=metadata_cache, + ) + return _data_transaction(metadata, addressing=addressing) + + return _read_stage( + key=("read", TelemetrySemantic.FORCE, "controls", selected_sensor), + main_index=MI_SENSOR_INFO, + sub_index=_SI_SELECTED_SENSOR, + length=_SENSOR_CONTROL_LENGTH, + on_data=consume_controls, + addressing=addressing, + next_factory=next_stage, + ) + + +def _build_metadata( + selected_sensor: int, + values: dict[str, bytes], + controls: bytes, +) -> TactileSensorMetadata: + prefix = values["prefix"] + sensor_type = _decode_padded_text(prefix[:_SI_DATA_UNIT], "sensor type") + if sensor_type is None: + raise ProtocolError("HOP tactile sensor type is empty") + data_unit = _decode_padded_text( + prefix[_SI_DATA_UNIT:_SI_DATA_RANGE], + "data unit", + ) + descriptor_data = values["descriptor"] + descriptor_text = _decode_descriptor(descriptor_data) + total_data_length = int.from_bytes(values["total_data_length"], "little") + if controls[0] != selected_sensor: + raise ProtocolError( + f"HOP selected sensor changed from {selected_sensor} to {controls[0]}" + ) + valid_data_position_mode = _decode_switch( + controls[_SI_VALID_DATA_POSITION_MODE - _SI_SELECTED_SENSOR], + "valid data position mode", + ) + rectangle_fill = _decode_switch( + controls[_SI_RECTANGLE_FILL - _SI_SELECTED_SENSOR], + "rectangle fill", + ) + + if descriptor_text is None: + if total_data_length != 0: + raise ProtocolError("HOP tactile sensor reports data without a descriptor") + if sensor_type != "NO_SENSOR": + raise ProtocolError("HOP installed tactile sensor has no descriptor") + descriptors: tuple[TactileFieldDescriptor, ...] = () + else: + if sensor_type == "NO_SENSOR": + raise ProtocolError("HOP NO_SENSOR metadata contains a descriptor") + descriptors = parse_tactile_descriptor( + descriptor_text, + total_data_length=total_data_length, + ) + if total_data_length > _MAX_SENSOR_DATA_LENGTH: + raise ProtocolError("HOP tactile data exceeds three 255-byte pages") + return TactileSensorMetadata( + selected_sensor=selected_sensor, + sensor_name=SENSOR_NAMES[selected_sensor - 1], + sensor_type=sensor_type, + data_unit=data_unit, + data_range=prefix[_SI_DATA_RANGE], + maximum_rows=prefix[_SI_MAXIMUM_ROWS], + maximum_columns=prefix[_SI_MAXIMUM_COLUMNS], + requested_rows=controls[_SI_REQUESTED_ROWS - _SI_SELECTED_SENSOR], + requested_columns=controls[_SI_REQUESTED_COLUMNS - _SI_SELECTED_SENSOR], + valid_data_position_mode=valid_data_position_mode, + rectangle_fill=rectangle_fill, + descriptor_text=descriptor_text, + field_descriptors=descriptors, + total_data_length=total_data_length, + ) + + +def _data_transaction( + metadata: dict[int, TactileSensorMetadata], + *, + addressing: HOPAddressing, +) -> ProtocolTransaction: + available = tuple( + selected_sensor + for selected_sensor in range(1, len(SENSOR_NAMES) + 1) + if metadata[selected_sensor].available + ) + if not available: + raise UnsupportedFirmwareError("HOP reports no available fingertip sensors") + # Keep five stable identities. An absent sensor has no bytes or fields; + # it is distinct from a present sensor that measured zero force. + samples = { + index: TactileSensorSample(metadata=value, raw_data=b"", fields=()) + for index, value in metadata.items() + if not value.available + } + return _data_select_transaction( + available, + 0, + metadata, + samples, + addressing=addressing, + ) + + +def _data_select_transaction( + available: tuple[int, ...], + sensor_position: int, + metadata: dict[int, TactileSensorMetadata], + samples: dict[int, TactileSensorSample], + *, + addressing: HOPAddressing, +) -> ProtocolTransaction: + selected_sensor = available[sensor_position] + payload = bytes((selected_sensor,)) + + def next_stage() -> ProtocolTransaction: + return _data_chunk_transaction( + available, + sensor_position, + metadata, + samples, + chunks=sensor_data_chunks(metadata[selected_sensor].total_data_length), + chunk_position=0, + raw_data=bytearray(metadata[selected_sensor].total_data_length), + global_offset=0, + addressing=addressing, + ) + + return _write_stage( + key=("read", TelemetrySemantic.FORCE, "select_data", selected_sensor), + sub_index=_SI_SELECTED_SENSOR, + payload=payload, + addressing=addressing, + next_factory=next_stage, + ) + + +def _data_chunk_transaction( + available: tuple[int, ...], + sensor_position: int, + metadata: dict[int, TactileSensorMetadata], + samples: dict[int, TactileSensorSample], + *, + chunks: tuple[tuple[int, int, int], ...], + chunk_position: int, + raw_data: bytearray, + global_offset: int, + addressing: HOPAddressing, +) -> ProtocolTransaction: + selected_sensor = available[sensor_position] + main_index, sub_index, length = chunks[chunk_position] + final_chunk = chunk_position + 1 == len(chunks) + final_sensor = sensor_position + 1 == len(available) + + def consume_chunk(data: bytes) -> SamplePayload | None: + raw_data[global_offset : global_offset + length] = data + if not final_chunk: + return None + sensor_metadata = metadata[selected_sensor] + exact_data = bytes(raw_data) + samples[selected_sensor] = TactileSensorSample( + metadata=sensor_metadata, + raw_data=exact_data, + fields=decode_tactile_fields( + sensor_metadata.field_descriptors, + exact_data, + ), + ) + if not final_sensor: + return None + return SamplePayload( + semantic=TelemetrySemantic.FORCE, + sensors=tuple(samples[index] for index in range(1, len(SENSOR_NAMES) + 1)), + origin=DataOrigin.DEVICE_RESPONSE, + ) + + def next_stage() -> ProtocolTransaction: + if not final_chunk: + return _data_chunk_transaction( + available, + sensor_position, + metadata, + samples, + chunks=chunks, + chunk_position=chunk_position + 1, + raw_data=raw_data, + global_offset=global_offset + length, + addressing=addressing, + ) + return _data_select_transaction( + available, + sensor_position + 1, + metadata, + samples, + addressing=addressing, + ) + + has_next = not (final_chunk and final_sensor) + return _read_stage( + key=( + "read", + TelemetrySemantic.FORCE, + "data", + selected_sensor, + chunk_position, + ), + main_index=main_index, + sub_index=sub_index, + length=length, + on_data=consume_chunk, + addressing=addressing, + next_factory=next_stage if has_next else None, + ) + + +def _read_stage( + *, + key: object, + main_index: int, + sub_index: int, + length: int, + on_data: Callable[[bytes], SamplePayload | None] | Callable[[bytes], None], + addressing: HOPAddressing, + next_factory: _NextFactory | None, +) -> ProtocolTransaction: + assembler = HOPRangeAssembler( + main_index, + sub_index=sub_index, + length=length, + rts=False, + response_id=addressing.response_id, + object_unavailable_is_unsupported=True, + ) + + def consume(message: object) -> ResponseProgress: + matched, complete = assembler.consume(message) + if complete is None: + return ResponseProgress(matched=matched) + result = on_data(complete) + return ResponseProgress( + matched=True, + complete=True, + result=result, + ) + + transaction = ProtocolTransaction( + key=key, + correlation_key=("hop", addressing.response_id), + request_messages=( + build_read_message( + main_index, + sub_index=sub_index, + length=length, + request_id=addressing.request_id, + brs=addressing.brs, + can_fd=addressing.can_fd, + ), + ), + response_timeout_s=_RESPONSE_TIMEOUT_S, + estimated_duration_s=_STAGE_ESTIMATE_S, + response_consumer=consume, + late_response_guard_s=_LATE_RESPONSE_GUARD_S, + next_transaction_factory=next_factory, + yield_after_response=next_factory is not None, + ) + return serialize_classic_read(transaction, addressing) + + +def _write_stage( + *, + key: object, + sub_index: int, + payload: bytes, + addressing: HOPAddressing, + next_factory: _NextFactory, +) -> ProtocolTransaction: + assembler = HOPRangeAssembler( + MI_SENSOR_INFO, + sub_index=sub_index, + length=len(payload), + rts=False, + response_id=addressing.response_id, + object_unavailable_is_unsupported=True, + ) + + def consume(message: object) -> ResponseProgress: + matched, complete = assembler.consume(message) + if complete is None: + return ResponseProgress(matched=matched) + if complete != payload: + raise ProtocolError("HOP tactile configuration response did not echo value") + return ResponseProgress(matched=True, complete=True) + + return ProtocolTransaction( + key=key, + correlation_key=("hop", addressing.response_id), + request_messages=( + build_write_message( + MI_SENSOR_INFO, + sub_index=sub_index, + payload=payload, + request_id=addressing.request_id, + brs=addressing.brs, + can_fd=addressing.can_fd, + ), + ), + response_timeout_s=_RESPONSE_TIMEOUT_S, + estimated_duration_s=_STAGE_ESTIMATE_S, + response_consumer=consume, + late_response_guard_s=_LATE_RESPONSE_GUARD_S, + next_transaction_factory=next_factory, + yield_after_response=True, + ) + + +def _decode_padded_text(data: bytes, label: str) -> str | None: + try: + value = data.split(b"\x00", 1)[0].decode("utf-8").strip() + except UnicodeDecodeError as error: + raise ProtocolError(f"HOP tactile {label} is not UTF-8") from error + return value or None + + +def _decode_descriptor(data: bytes) -> str | None: + if not data: + return None + try: + value = data.decode("ascii") + except UnicodeDecodeError as error: + raise ProtocolError("HOP tactile descriptor is not ASCII") from error + if not value or "\x00" in value or value.strip() != value: + raise ProtocolError("HOP tactile descriptor contains invalid padding") + return value + + +def _decode_switch(value: int, label: str) -> bool: + if value not in {0, 1}: + raise ProtocolError(f"HOP tactile {label} must be zero or one") + return bool(value) + + +__all__ = [ + "SENSOR_NAMES", + "decode_tactile_fields", + "parse_tactile_descriptor", + "sensor_data_chunks", + "tactile_transaction", +] diff --git a/src/linkerbot_hand/protocols/hop/six_axis.py b/src/linkerbot_hand/protocols/hop/six_axis.py new file mode 100644 index 0000000..92162e9 --- /dev/null +++ b/src/linkerbot_hand/protocols/hop/six_axis.py @@ -0,0 +1,261 @@ +"""Six-axis HOP backend, using installed V36 positions without hole writes.""" + +from __future__ import annotations + +from ...api.enums import DataOrigin, HandSide +from ...api.errors import ProtocolError, UnsupportedOperationError +from ...api.readings import RawNumber +from ...capabilities.types import CommandSemantic, DeviceCapabilities, TelemetrySemantic +from ..base import ProtocolTransaction, ResponseProgress, SamplePayload +from .backend import HOPBackend +from .framing import ( + HOPAddressing, + build_read_message, + build_write_message, +) +from .identity import identity_transaction +from .objects import decode_fault_statuses +from .response import HOPRangeAssembler + +# The order here is public API order, independent of logical wire position. +POSITIONS = (10, 5, 11, 12, 13, 14) +_READS = { + TelemetrySemantic.ANGLES: (0x01, False), + TelemetrySemantic.MEASURED_SPEEDS: (0x02, False), + TelemetrySemantic.CONFIGURED_SPEEDS: (0x02, True), + TelemetrySemantic.CONFIGURED_ACCELERATIONS: (0x03, True), + TelemetrySemantic.CURRENTS: (0x04, False), + TelemetrySemantic.CURRENT_LIMITS: (0x04, True), + TelemetrySemantic.VOLTAGES: (0x05, False), + TelemetrySemantic.TORQUES: (0x06, False), + TelemetrySemantic.TORQUES_LIMITS: (0x06, True), + TelemetrySemantic.TEMPERATURES: (0x07, False), + TelemetrySemantic.MOTION_TIMES: (0x08, True), + TelemetrySemantic.STALL_DETECTION_TIMES: (0x09, True), + TelemetrySemantic.STALL_THRESHOLDS: (0x0A, True), + TelemetrySemantic.STALL_HOLD_CURRENTS: (0x0B, True), + TelemetrySemantic.FAULTS: (0x0C, False), + TelemetrySemantic.POSITION_KP: (0x21, True), + TelemetrySemantic.POSITION_KD: (0x23, True), +} +_COMMANDS = { + CommandSemantic.ANGLES: (0x01, False), + CommandSemantic.SPEEDS: (0x02, True), + CommandSemantic.ACCELERATIONS: (0x03, True), + CommandSemantic.CURRENT_LIMITS: (0x04, True), + CommandSemantic.TORQUES: (0x06, False), + CommandSemantic.TORQUES_LIMITS: (0x06, True), + CommandSemantic.MOTION_TIMES: (0x08, True), + CommandSemantic.STALL_DETECTION_TIMES: (0x09, True), + CommandSemantic.STALL_THRESHOLDS: (0x0A, True), + CommandSemantic.STALL_HOLD_CURRENTS: (0x0B, True), + CommandSemantic.POSITION_KP: (0x21, True), + CommandSemantic.POSITION_KD: (0x23, True), +} + + +def vector_transaction( + main_index: int, + rts: bool, + addressing: HOPAddressing, + capabilities: DeviceCapabilities, + *, + semantic: TelemetrySemantic | None = None, + values: tuple[RawNumber, ...] | None = None, +) -> ProtocolTransaction: + """Publish/ACK only after both ranges; writes are not mechanically atomic.""" + width = 2 if main_index in (0x21, 0x23) else 1 + received: dict[int, int] = {} + spans = ((5, 1), (10, 5)) + + def stage(index: int) -> ProtocolTransaction: + start, count = spans[index] + assembler = HOPRangeAssembler( + main_index, + sub_index=start * width, + length=count * width, + rts=rts, + response_id=addressing.response_id, + ) + if values is None: + message = build_read_message( + main_index, + sub_index=start * width, + length=count * width, + rts=rts, + request_id=addressing.request_id, + brs=addressing.brs, + can_fd=addressing.can_fd, + ) + else: + payload = b"".join( + int(values[POSITIONS.index(pos)]).to_bytes(width, "little") + for pos in range(start, start + count) + ) + message = build_write_message( + main_index, + sub_index=start * width, + payload=payload, + rts=rts, + request_id=addressing.request_id, + brs=addressing.brs, + can_fd=addressing.can_fd, + ) + + def consume(message: object) -> ResponseProgress: + matched, complete = assembler.consume(message) + if complete is None: + return ResponseProgress(matched=matched) + received.update( + ( + start + n, + int.from_bytes(complete[n * width : (n + 1) * width], "little"), + ) + for n in range(count) + ) + result = None + if index == len(spans) - 1 and semantic is not None: + raw = tuple(received[pos] for pos in POSITIONS) + telemetry = capabilities.telemetry_channel(semantic) + result = SamplePayload( + semantic=semantic, + raw=raw, + origin=DataOrigin.DEVICE_COMMAND_FEEDBACK + if values is not None + else ( + DataOrigin.DEVICE_CONFIG_RESPONSE + if rts + else DataOrigin.DEVICE_RESPONSE + ), + within_declared_range=( + telemetry is None + or telemetry.raw_range is None + or all(telemetry.raw_range.contains(v) for v in raw) + ), + fault_statuses=decode_fault_statuses(raw) + if semantic is TelemetrySemantic.FAULTS + else (), + ) + return ResponseProgress( + matched=True, + complete=True, + result=result, + acknowledged=values is not None, + ) + + return ProtocolTransaction( + key=("hop_vector", main_index, rts), + correlation_key=("hop", addressing.response_id), + request_messages=(message,), + response_timeout_s=0.1, + estimated_duration_s=0.004, + response_consumer=consume, + response_acknowledges_command=values is not None, + late_response_guard_s=0.01, + next_transaction_factory=(lambda: stage(index + 1)) if index == 0 else None, + ) + + return stage(0) + + +class HOPSixAxisBackend(HOPBackend): + """Reuse shared HOP identity/sensors and override model-specific objects.""" + + def __init__( + self, + model: str, + side: HandSide, + *, + request_id: int | None = None, + brs: bool = False, + can_fd: bool = True, + ) -> None: + super().__init__( + side, + request_id=1 if request_id is None else request_id, + brs=brs, + can_fd=can_fd, + ) + self._model = model + + def identity_transaction( + self, capabilities: DeviceCapabilities + ) -> ProtocolTransaction: + if capabilities.model != self._model or capabilities.side is not self._side: + raise ProtocolError("HOP capability identity changed") + if not capabilities.identity.verified: + self._sensor_metadata.clear() + self._identity_fingerprint = None + return identity_transaction( + capabilities, self._addressing, on_identity=self._observe_identity + ) + + def command_transaction( + self, + semantic: CommandSemantic, + values: tuple[RawNumber, ...], + capabilities: DeviceCapabilities, + ) -> ProtocolTransaction: + command = capabilities.command(semantic) + if command is None or semantic not in _COMMANDS: + raise UnsupportedOperationError( + f"{self._model} HOP does not support {semantic.value}" + ) + if len(values) != 6 or any( + type(v) is not int or not command.range_for(n).contains(v) + for n, v in enumerate(values) + ): + raise ProtocolError( + "HOP six-axis commands require six raw integers in 0..255" + ) + mi, rts = _COMMANDS[semantic] + return vector_transaction( + mi, + rts, + self._addressing, + capabilities, + values=values, + semantic=TelemetrySemantic.ANGLES + if semantic is CommandSemantic.ANGLES + else None, + ) + + def read_transaction( + self, semantic: TelemetrySemantic, capabilities: DeviceCapabilities + ) -> ProtocolTransaction: + if semantic is TelemetrySemantic.FORCE: + return self.sensor_transaction(capabilities) + if capabilities.telemetry_channel(semantic) is None or semantic not in _READS: + raise UnsupportedOperationError( + f"{self._model} HOP does not support {semantic.value}" + ) + mi, rts = _READS[semantic] + return vector_transaction( + mi, rts, self._addressing, capabilities, semantic=semantic + ) + + def operation_transaction( + self, operation: str, capabilities: DeviceCapabilities + ) -> ProtocolTransaction: + if operation != "save_parameters": + raise UnsupportedOperationError( + f"{self._model} HOP does not support {operation}" + ) + from .management import save_transaction + + return save_transaction(self._addressing) + + def can_configuration_transaction(self) -> ProtocolTransaction: + from .management import configuration_transaction + + return configuration_transaction(self._addressing) + + def can_configuration_write( + self, sub_index: int, payload: bytes + ) -> ProtocolTransaction: + from .management import configuration_write + + return configuration_write(self._addressing, sub_index, payload) + + +__all__ = ["POSITIONS", "HOPSixAxisBackend", "vector_transaction"] diff --git a/src/linkerbot_hand/protocols/o30/codec.py b/src/linkerbot_hand/protocols/o30/codec.py index d7b7453..d8fe506 100644 --- a/src/linkerbot_hand/protocols/o30/codec.py +++ b/src/linkerbot_hand/protocols/o30/codec.py @@ -155,19 +155,9 @@ def decode_fault_statuses(raw: tuple[int, ...]) -> tuple[JointFaultStatus, ...]: raise ProtocolError( f"O30 faults must contain {PUBLIC_JOINT_COUNT} unsigned bytes" ) - return tuple( - JointFaultStatus( - raw=value, - hand_layer_stall=bool(value & FAULT_HAND_LAYER_STALL), - actuator_layer_stall=bool(value & FAULT_ACTUATOR_LAYER_STALL), - over_current=bool(value & FAULT_OVER_CURRENT), - over_temperature=bool(value & FAULT_OVER_TEMPERATURE), - actuator_abnormal=bool(value & FAULT_ACTUATOR_ABNORMAL), - actuator_offline=bool(value & FAULT_ACTUATOR_OFFLINE), - reserved_bits=value & FAULT_RESERVED, - ) - for value in raw - ) + from ..hop.objects import decode_fault_statuses as decode_hop_faults + + return decode_hop_faults(raw) __all__ = [ diff --git a/src/linkerbot_hand/protocols/o30/control.py b/src/linkerbot_hand/protocols/o30/control.py index 06f6bc4..2d5a044 100644 --- a/src/linkerbot_hand/protocols/o30/control.py +++ b/src/linkerbot_hand/protocols/o30/control.py @@ -61,7 +61,7 @@ def command_transaction( capabilities: DeviceCapabilities, addressing: HOPAddressing, ) -> ProtocolTransaction: - """Build one compact write whose correlated HOP response is the ACK.""" + """Build one FD write or five response-gated classic CAN range writes.""" try: command = _COMMAND_OBJECTS[semantic] @@ -72,57 +72,68 @@ def command_transaction( main_index = command.main_index raw = _validated_values(values, semantic, capabilities) payload = encode_v25(raw) - assembler = HOPRangeAssembler( - main_index, - sub_index=0, - length=WIRE_SLOT_COUNT, - rts=command.rts, - response_id=addressing.response_id, - ) - - def consume(message: object) -> ResponseProgress: - matched, complete = assembler.consume(message) - if complete is None: - return ResponseProgress(matched=matched) - feedback = None - if semantic is CommandSemantic.ANGLES: - # RTS=0 selects actual state, which can differ from the target - # while the hand is moving. ACK is not a mechanical arrival test. - raw = decode_v25(complete) - feedback = SamplePayload( - semantic=TelemetrySemantic.ANGLES, - raw=raw, - origin=DataOrigin.DEVICE_COMMAND_FEEDBACK, - within_declared_range=all( - joint.raw_angle_range.contains(value) - for joint, value in zip(capabilities.joints, raw, strict=True) - ), - ) - return ResponseProgress( - matched=True, - complete=True, - acknowledged=True, - result=feedback, + fragment_size = WIRE_SLOT_COUNT if addressing.can_fd else 5 + feedback_bytes = bytearray(WIRE_SLOT_COUNT) + + def stage(start: int) -> ProtocolTransaction: + end = min(start + fragment_size, WIRE_SLOT_COUNT) + final = end == WIRE_SLOT_COUNT + assembler = HOPRangeAssembler( + main_index, + sub_index=start, + length=end - start, + rts=command.rts, + response_id=addressing.response_id, ) - return ProtocolTransaction( - key=("command", semantic), - correlation_key=("o30_hop", addressing.response_id), - request_messages=( - build_write_message( - main_index, - sub_index=0, - payload=payload, - rts=command.rts, - request_id=addressing.request_id, + def consume(message: object) -> ResponseProgress: + matched, complete = assembler.consume(message) + if complete is None: + return ResponseProgress(matched=matched) + feedback_bytes[start:end] = complete + feedback = None + if final and semantic is CommandSemantic.ANGLES: + # Publish a complete vector only after all ranges acknowledge. + # Segmented writes and the returned states are not simultaneous. + returned = decode_v25(bytes(feedback_bytes)) + feedback = SamplePayload( + semantic=TelemetrySemantic.ANGLES, + raw=returned, + origin=DataOrigin.DEVICE_COMMAND_FEEDBACK, + within_declared_range=all( + joint.raw_angle_range.contains(value) + for joint, value in zip( + capabilities.joints, returned, strict=True + ) + ), + ) + return ResponseProgress( + matched=True, complete=True, acknowledged=True, result=feedback + ) + + return ProtocolTransaction( + key=("command", semantic), + correlation_key=("hop", addressing.response_id), + request_messages=( + build_write_message( + main_index, + sub_index=start, + payload=payload[start:end], + rts=command.rts, + request_id=addressing.request_id, + brs=addressing.brs, + can_fd=addressing.can_fd, + ), ), - ), - response_timeout_s=0.1, - estimated_duration_s=0.003, - response_consumer=consume, - response_acknowledges_command=True, - late_response_guard_s=0.01, - ) + response_timeout_s=0.1, + estimated_duration_s=0.003, + response_consumer=consume, + response_acknowledges_command=True, + late_response_guard_s=0.01, + next_transaction_factory=None if final else lambda: stage(end), + ) + + return stage(0) def _validated_values( diff --git a/src/linkerbot_hand/protocols/o30/framing.py b/src/linkerbot_hand/protocols/o30/framing.py index bf39bf8..191a207 100644 --- a/src/linkerbot_hand/protocols/o30/framing.py +++ b/src/linkerbot_hand/protocols/o30/framing.py @@ -1,178 +1,28 @@ -"""Transport-neutral HOP framing for the O30 CAN FD endpoint.""" - -from __future__ import annotations - -from dataclasses import dataclass - -from linkerbot_transport import CANRouteKey -from linkerbot_transport.canfd import CANFDMessage - -from ...api.enums import HandSide -from ...api.errors import ProtocolError, ValidationError - -# O30 defaults to a standard-frame CAN ID pair per side. The response -# ID is the request ID with the top standard-frame bit (0x400) set. -RIGHT_REQUEST_ID = 0x001 -RIGHT_RESPONSE_ID = 0x401 -LEFT_REQUEST_ID = 0x002 -LEFT_RESPONSE_ID = 0x402 -# Backwards-friendly aliases for the right-hand pair (used by discovery/tests). -REQUEST_ID = RIGHT_REQUEST_ID -RESPONSE_ID = RIGHT_RESPONSE_ID - -FRAME_TYPE = 0x04 -ERROR_MAIN_INDEX = 0x4F -RTS_MASK = 0x80 -MAIN_INDEX_MASK = 0x7F -MAX_FRAGMENT_LENGTH = 61 -_STANDARD_ID_MAX = 0x7FF - - -@dataclass(frozen=True, slots=True) -class HOPAddressing: - """One O30 endpoint's request/response standard-frame CAN ID pair.""" - - request_id: int - response_id: int - - def __post_init__(self) -> None: - for value, name in ( - (self.request_id, "request_id"), - (self.response_id, "response_id"), - ): - if type(value) is not int or not 0 <= value <= _STANDARD_ID_MAX: - raise ValidationError(f"HOP {name} must be a standard CAN ID") - if self.request_id > 0x3FE: - raise ValidationError("HOP unicast request_id must be within 0x000..0x3FE") - if self.response_id != self.request_id | 0x400: - raise ValidationError("HOP response_id must be request_id | 0x400") - - -RIGHT_ADDRESSING = HOPAddressing(RIGHT_REQUEST_ID, RIGHT_RESPONSE_ID) -LEFT_ADDRESSING = HOPAddressing(LEFT_REQUEST_ID, LEFT_RESPONSE_ID) - - -def addressing_for_side(side: HandSide) -> HOPAddressing: - """Return the default CAN ID pair for ``side``; IDs may be configured.""" - - if side is HandSide.RIGHT: - return RIGHT_ADDRESSING - if side is HandSide.LEFT: - return LEFT_ADDRESSING - raise ValidationError("O30 addressing requires a left or right side") - - -@dataclass(frozen=True, slots=True) -class HOPFrame: - """One decoded HOP response with CAN FD padding removed.""" - - main_index: int - sub_index: int - payload: bytes - rts: bool - - -def response_route(response_id: int) -> CANRouteKey: - """Return the single standard-ID route consumed by an O30 endpoint.""" - - return CANRouteKey(response_id, False) - - -def build_read_message( - main_index: int, - *, - sub_index: int, - length: int, - request_id: int, - rts: bool = False, -) -> CANFDMessage: - """Build one three-byte HOP object-range read.""" - - _validate_object_range(main_index, sub_index, length) - if not isinstance(rts, bool): - raise ValidationError("rts must be bool") - control = main_index | (RTS_MASK if rts else 0) - return CANFDMessage( - arbitration_id=request_id, - data=bytes((control, sub_index, length)), - is_extended_id=False, - frame_type=FRAME_TYPE, - ) - - -def build_write_message( - main_index: int, - *, - sub_index: int, - payload: bytes, - request_id: int, - rts: bool = False, -) -> CANFDMessage: - """Build one single-frame HOP object-range write.""" - - if not isinstance(payload, bytes) or not payload: - raise ValidationError("HOP write payload must be non-empty bytes") - if len(payload) > MAX_FRAGMENT_LENGTH: - raise ValidationError( - f"HOP write payload cannot exceed {MAX_FRAGMENT_LENGTH} bytes" - ) - if not isinstance(rts, bool): - raise ValidationError("rts must be bool") - _validate_object_range(main_index, sub_index, len(payload)) - control = main_index | (RTS_MASK if rts else 0) - return CANFDMessage( - arbitration_id=request_id, - data=bytes((control, sub_index, len(payload))) + payload, - is_extended_id=False, - frame_type=FRAME_TYPE, - ) - - -def matching_response_frame(message: object, response_id: int) -> HOPFrame | None: - """Decode one exact endpoint response or ignore unrelated CAN traffic.""" - - if ( - not isinstance(message, CANFDMessage) - or message.is_extended_id - or message.arbitration_id != response_id - ): - return None - return decode_frame(message.data) - - -def decode_frame(data: bytes) -> HOPFrame: - """Decode one response fragment using EDL instead of padded DLC length.""" - - if not isinstance(data, bytes) or len(data) < 3: - raise ProtocolError("O30 HOP response is shorter than its header") - control, sub_index, effective_length = data[:3] - required = 3 + effective_length - if len(data) < required: - raise ProtocolError( - f"O30 HOP response declares {effective_length} payload bytes but " - f"contains {max(0, len(data) - 3)}" - ) - return HOPFrame( - main_index=control & MAIN_INDEX_MASK, - sub_index=sub_index, - payload=data[3:required], - rts=bool(control & RTS_MASK), - ) - - -def _validate_object_range(main_index: int, sub_index: int, length: int) -> None: - for value, name, minimum, maximum in ( - (main_index, "main_index", 0, MAIN_INDEX_MASK), - (sub_index, "sub_index", 0, 0xFF), - (length, "length", 1, 0xFF), - ): - if type(value) is not int: - raise ValidationError(f"{name} must be int") - if value < minimum or value > maximum: - raise ValidationError(f"{name} must be within {minimum}..{maximum}") - if sub_index + length > 0x100: - raise ValidationError("sub_index + length cannot exceed 256") - +"""Compatibility exports for the shared HOP framing implementation.""" + +from ..hop.framing import ( + ERROR_MAIN_INDEX, + FRAME_TYPE, + LEFT_ADDRESSING, + LEFT_REQUEST_ID, + LEFT_RESPONSE_ID, + MAIN_INDEX_MASK, + MAX_FRAGMENT_LENGTH, + REQUEST_ID, + RESPONSE_ID, + RIGHT_ADDRESSING, + RIGHT_REQUEST_ID, + RIGHT_RESPONSE_ID, + RTS_MASK, + HOPAddressing, + HOPFrame, + addressing_for_side, + build_read_message, + build_write_message, + decode_frame, + matching_response_frame, + response_route, +) __all__ = [ "ERROR_MAIN_INDEX", diff --git a/src/linkerbot_hand/protocols/o30/identity.py b/src/linkerbot_hand/protocols/o30/identity.py index c3be861..9b97a1d 100644 --- a/src/linkerbot_hand/protocols/o30/identity.py +++ b/src/linkerbot_hand/protocols/o30/identity.py @@ -1,431 +1,5 @@ -"""Response-gated O30 identity with deployed and documented HOP layouts.""" - -from __future__ import annotations - -import re -from collections.abc import Callable -from dataclasses import dataclass, replace - -from ...api.device_info import DeviceInfo, VersionField -from ...api.enums import HandSide -from ...api.errors import ( - IdentityMismatchError, - ProtocolError, - UnsupportedFirmwareError, -) -from ...api.readings import SampleMeta -from ...capabilities.types import DeviceCapabilities -from ..base import IdentityResult, ProtocolTransaction, ResponseProgress -from .codec import MI_PRODUCT_INFO -from .framing import HOPAddressing, build_read_message -from .response import HOPRangeAssembler - -_EXPECTED_PROTOCOL_NAME = "HOP" -_VERSION_PATTERN = re.compile(r"^[VM]?\d+(?:\.\d+){1,2}$", re.IGNORECASE) -_DATE_PREFIX_PATTERN = re.compile(r"^\d{4}-\d{2}") - - -@dataclass(frozen=True, slots=True) -class _IdentityField: - name: str - sub_index: int - length: int - required: bool = False - - -_COMMON_FIELDS = ( - _IdentityField("product_model", 0x00, 0x10, required=True), - _IdentityField("power_supply_range", 0x10, 0x10), - _IdentityField("microcontroller_uid", 0x20, 0x19), - _IdentityField("device_uid", 0x39, 0x20, required=True), - _IdentityField("protocol_name", 0x59, 0x08, required=True), - _IdentityField("protocol_version", 0x61, 0x08, required=True), - _IdentityField("board_layout_probe", 0x69, 0x10, required=True), - _IdentityField("version_layout_probe", 0x99, 0x08), -) - -_DEPLOYED_FIELDS = ( - _IdentityField("control_board_version", 0x79, 0x08), - _IdentityField("bootloader_version", 0x81, 0x08), - _IdentityField("application_version", 0x89, 0x08, required=True), - _IdentityField("mechanical_version", 0x91, 0x08), - _IdentityField("build_time", 0x99, 0x20), - _IdentityField("supported_interfaces", 0xB9, 0x20), - _IdentityField("reported_side", 0xD9, 0x08), -) - -_DOCUMENTED_FIELDS = ( - _IdentityField("adapter_board_version", 0x79, 0x10), - _IdentityField("control_board_version", 0x89, 0x10), - _IdentityField("application_version", 0xA1, 0x08, required=True), - _IdentityField("mechanical_version", 0xA9, 0x08), - _IdentityField("build_time", 0xB1, 0x20), - _IdentityField("supported_interfaces", 0xD1, 0x20), - _IdentityField("reported_side", 0xF1, 0x08), -) - - -def o30_identity_transaction( - capabilities: DeviceCapabilities, - addressing: HOPAddressing, - *, - on_identity: Callable[[IdentityResult], None] | None = None, -) -> ProtocolTransaction: - """Read identity without imposing a firmware-version connection gate.""" - - if capabilities.model != "O30": - raise ProtocolError("O30 identity transaction requires O30 capabilities") - values: dict[str, bytes] = {} - return _field_transaction( - _COMMON_FIELDS, - 0, - capabilities, - values, - addressing, - next_factory=lambda: _layout_transaction( - capabilities, values, addressing, on_identity=on_identity - ), - ) - - -def _layout_transaction( - capabilities: DeviceCapabilities, - values: dict[str, bytes], - addressing: HOPAddressing, - *, - on_identity: Callable[[IdentityResult], None] | None, -) -> ProtocolTransaction: - layout = _select_layout(values) - fields = _DEPLOYED_FIELDS if layout == "deployed" else _DOCUMENTED_FIELDS - - def result_factory() -> IdentityResult: - result = _identity_result(capabilities, values, layout=layout) - if on_identity is not None: - on_identity(result) - return result - - return _field_transaction( - fields, - 0, - capabilities, - values, - addressing, - result_factory=result_factory, - ) - - -def _field_transaction( - fields: tuple[_IdentityField, ...], - index: int, - capabilities: DeviceCapabilities, - values: dict[str, bytes], - addressing: HOPAddressing, - *, - next_factory: Callable[[], ProtocolTransaction] | None = None, - result_factory: Callable[[], IdentityResult] | None = None, -) -> ProtocolTransaction: - field = fields[index] - # In the compact layout, the probe is the first eight bytes of build_time. - # Reuse those bytes instead of asking the device twice. - prefix = ( - values["version_layout_probe"] - if field.name == "build_time" and field.sub_index == 0x99 - else b"" - ) - request_start = field.sub_index + len(prefix) - request_length = field.length - len(prefix) - assembler = HOPRangeAssembler( - MI_PRODUCT_INFO, - sub_index=request_start, - length=request_length, - rts=False, - response_id=addressing.response_id, - ) - final_field = index + 1 == len(fields) - - def build_next() -> ProtocolTransaction | None: - if not final_field: - return _field_transaction( - fields, - index + 1, - capabilities, - values, - addressing, - next_factory=next_factory, - result_factory=result_factory, - ) - if next_factory is not None: - return next_factory() - return None - - def consume(message: object) -> ResponseProgress: - matched, complete = assembler.consume(message) - if complete is None: - return ResponseProgress(matched=matched) - complete = prefix + complete - text = _decode_text(complete, field.name) - if field.required and text is None: - raise ProtocolError(f"O30 {field.name} is empty") - if text is not None: - _validate_identity_field(field.name, text) - values[field.name] = complete - result = ( - result_factory() - if final_field and next_factory is None and result_factory is not None - else None - ) - return ResponseProgress(matched=True, complete=True, result=result) - - has_next = not final_field or next_factory is not None - return ProtocolTransaction( - key=("identity", "o30", field.name), - correlation_key=("o30_hop", addressing.response_id), - request_messages=( - build_read_message( - MI_PRODUCT_INFO, - sub_index=request_start, - length=request_length, - request_id=addressing.request_id, - ), - ), - response_timeout_s=0.1, - estimated_duration_s=0.003, - response_consumer=consume, - late_response_guard_s=0.01, - next_transaction_factory=build_next if has_next else None, - ) - - -def _select_layout(values: dict[str, bytes]) -> str: - board_probe = values["board_layout_probe"] - version_probe = _decode_text( - values["version_layout_probe"], - "version_layout_probe", - ) - # Splitting a documented UTF-8 board name at byte eight may cut a code - # point. Only the observed layout contains two NUL-padded ASCII versions. - second_board_bytes = board_probe[8:].split(b"\x00", 1)[0] - second_board_field = ( - second_board_bytes.decode("ascii") if second_board_bytes.isascii() else None - ) - if _looks_like_version(second_board_field) or ( - version_probe is not None - and _DATE_PREFIX_PATTERN.match(version_probe) is not None - ): - return "deployed" - return "documented" - - -def _identity_result( - declared: DeviceCapabilities, - values: dict[str, bytes], - *, - layout: str, -) -> IdentityResult: - product_model = _required_text(values, "product_model") - device_uid = _required_text(values, "device_uid") - protocol_name = _required_text(values, "protocol_name") - protocol_version = _required_text(values, "protocol_version") - board_probe = values["board_layout_probe"] - - if layout == "deployed": - interface_text = _decode_text( - board_probe[:8], - "interface_board_version", - ) - adapter_text = _decode_text( - board_probe[8:], - "adapter_board_version", - ) - interface_raw = board_probe[:8] - adapter_raw = board_probe[8:] - bootloader_raw = values["bootloader_version"] - else: - interface_text = _decode_text( - board_probe, - "interface_board_version", - ) - adapter_text = _decode_text( - values["adapter_board_version"], - "adapter_board_version", - ) - interface_raw = board_probe - adapter_raw = values["adapter_board_version"] - bootloader_raw = values["version_layout_probe"] - - control_text = _optional_text(values, "control_board_version") - bootloader_text = _decode_text(bootloader_raw, "bootloader_version") - application_text = _required_text(values, "application_version") - mechanical_text = _optional_text(values, "mechanical_version") - build_time = _optional_text(values, "build_time") - supported_interfaces = _optional_text(values, "supported_interfaces") - power_supply_range = _optional_text(values, "power_supply_range") - microcontroller_uid = _optional_text(values, "microcontroller_uid") - - interface = _version_or_none(interface_raw, interface_text) - adapter = _version_or_none(adapter_raw, adapter_text) - control = _version_or_none(values["control_board_version"], control_text) - bootloader = _version_or_none(bootloader_raw, bootloader_text) - firmware = _version(values["application_version"], application_text) - mechanical = _version_or_none(values["mechanical_version"], mechanical_text) - - side_field = _optional_text(values, "reported_side") - field_side = _parse_side(side_field) - product_side = _side_from_product_model(product_model) - if ( - field_side is not None - and product_side is not None - and field_side is not product_side - ): - raise IdentityMismatchError("O30 product model and reported side disagree") - reported_side = field_side or product_side - resolved_side = reported_side or declared.side - - verified_identity = replace( - declared.identity, - side=resolved_side, - verified=True, - firmware=firmware.text, - hardware_revision=None if interface is None else interface.text, - mechanical_revision=None if mechanical is None else mechanical.text, - sensor_variant=None, - serial_number=device_uid, - product_code=product_model, - protocol_name=protocol_name, - protocol_version=protocol_version, - power_supply_range=power_supply_range, - microcontroller_uid=microcontroller_uid, - adapter_board_version=None if adapter is None else adapter.text, - control_board_version=None if control is None else control.text, - bootloader_version=None if bootloader is None else bootloader.text, - build_time=build_time, - supported_interfaces=supported_interfaces, - reported_side=reported_side, - ) - verified = replace(declared, identity=verified_identity) - info = DeviceInfo( - model=verified.model, - serial_number=device_uid, - pcb_version=interface, - firmware_version=firmware, - mechanical_version=mechanical, - product_code=product_model, - sensor_variant=None, - side=resolved_side, - raw_identity=_raw_identity(values, layout=layout), - meta=SampleMeta.unavailable(), - adapter_board_version=adapter, - control_board_version=control, - bootloader_version=bootloader, - build_time=build_time, - supported_interfaces=supported_interfaces, - reported_side=reported_side, - protocol_name=protocol_name, - protocol_version=protocol_version, - power_supply_range=power_supply_range, - microcontroller_uid=microcontroller_uid, - ) - fingerprint = ( - product_model, - device_uid, - protocol_name, - protocol_version, - None if interface is None else interface.text, - None if adapter is None else adapter.text, - None if control is None else control.text, - None if bootloader is None else bootloader.text, - firmware.text, - None if mechanical is None else mechanical.text, - None if reported_side is None else reported_side.value, - ) - return IdentityResult( - capabilities=verified, - device_info=info, - fingerprint=fingerprint, - ) - - -def _raw_identity(values: dict[str, bytes], *, layout: str) -> bytes: - fields = _COMMON_FIELDS + ( - _DEPLOYED_FIELDS if layout == "deployed" else _DOCUMENTED_FIELDS - ) - length = 0xE1 if layout == "deployed" else 0xF9 - raw = bytearray(length) - for field in fields: - data = values[field.name] - raw[field.sub_index : field.sub_index + field.length] = data - return bytes(raw) - - -def _validate_identity_field(name: str, value: str) -> None: - normalized = value.casefold() - if name == "product_model" and not ( - normalized in {"o30", "o30i"} or normalized.startswith(("o30-", "o30i-")) - ): - raise IdentityMismatchError(f"expected O30 product model, got {value!r}") - if name == "protocol_name" and value.upper() != _EXPECTED_PROTOCOL_NAME: - raise UnsupportedFirmwareError(f"O30 protocol {value!r} is unsupported") - - -def _required_text(values: dict[str, bytes], name: str) -> str: - value = _optional_text(values, name) - if value is None: - raise ProtocolError(f"O30 {name} is empty") - return value - - -def _optional_text(values: dict[str, bytes], name: str) -> str | None: - return _decode_text(values[name], name) - - -def _decode_text(data: bytes, name: str) -> str | None: - try: - value = data.split(b"\x00", 1)[0].decode("utf-8").strip() - except UnicodeDecodeError as error: - raise ProtocolError(f"O30 {name} is not UTF-8") from error - return value or None - - -def _looks_like_version(value: str | None) -> bool: - return value is not None and _VERSION_PATTERN.fullmatch(value) is not None - - -def _parse_side(value: str | None) -> HandSide | None: - if value is None: - return None - normalized = value.strip().casefold() - if normalized in {"l", "left"}: - return HandSide.LEFT - if normalized in {"r", "right"}: - return HandSide.RIGHT - raise ProtocolError(f"O30 reported side {value!r} is invalid") - - -def _side_from_product_model(value: str) -> HandSide | None: - parts = value.split("-") - if len(parts) < 4: - return None - return {"L": HandSide.LEFT, "R": HandSide.RIGHT}.get(parts[3].upper()) - - -def _version(raw: bytes, text: str) -> VersionField: - semantic = text.removeprefix("V").removeprefix("v") - semantic = semantic.removeprefix("M").removeprefix("m") - parts = semantic.split(".") - if len(parts) == 3 and all(part.isdigit() for part in parts): - major, minor, patch = (int(part) for part in parts) - return VersionField( - raw=raw, - text=text, - major=major, - minor=minor, - patch=patch, - ) - return VersionField(raw=raw, text=text) - - -def _version_or_none(raw: bytes, text: str | None) -> VersionField | None: - return None if text is None else _version(raw, text) +"""Compatibility entry for O30 identity using the shared HOP implementation.""" +from ..hop.identity import identity_transaction as o30_identity_transaction __all__ = ["o30_identity_transaction"] diff --git a/src/linkerbot_hand/protocols/o30/response.py b/src/linkerbot_hand/protocols/o30/response.py index b4cfc1c..c009dda 100644 --- a/src/linkerbot_hand/protocols/o30/response.py +++ b/src/linkerbot_hand/protocols/o30/response.py @@ -1,99 +1,7 @@ -"""Range-aware HOP response assembly and device-error classification.""" - -from __future__ import annotations - -from ...api.errors import ProtocolError, UnsupportedFirmwareError -from .framing import ERROR_MAIN_INDEX, matching_response_frame - -_DEVICE_ERROR_MESSAGES = { - 0x01: "main index does not exist", - 0x02: "sub-index does not exist", - 0x04: "register is not writable", - 0x08: "data length mismatch", - 0x10: "permission denied", - 0x20: "returned data contains zero padding", -} - - -class HOPRangeAssembler: - """Assemble one serialized HOP object range by absolute sub-index.""" - - def __init__( - self, - main_index: int, - *, - sub_index: int, - length: int, - rts: bool, - response_id: int, - object_unavailable_is_unsupported: bool = False, - ) -> None: - self._main_index = main_index - self._sub_index = sub_index - self._length = length - self._rts = rts - self._response_id = response_id - self._object_unavailable_is_unsupported = object_unavailable_is_unsupported - self._buffer = bytearray(length) - self._received = bytearray(length) - - def consume(self, message: object) -> tuple[bool, bytes | None]: - """Return ``(matched, complete_payload)`` for one routed message.""" - - frame = matching_response_frame(message, self._response_id) - if frame is None: - return False, None - if ( - frame.main_index == ERROR_MAIN_INDEX - and self._main_index != ERROR_MAIN_INDEX - ): - if len(frame.payload) != 1: - raise ProtocolError("O30 HOP error response must contain one byte") - code = frame.payload[0] - if code == 0: - return False, None - descriptions = [ - message for bit, message in _DEVICE_ERROR_MESSAGES.items() if code & bit - ] - if code & ~0x3F: - descriptions.append(f"unknown flags 0x{code & ~0x3F:02X}") - description = "; ".join(descriptions) - if ( - self._object_unavailable_is_unsupported - and code & 0x03 - and not code & ~0x23 - ): - raise UnsupportedFirmwareError( - f"O30 HOP object 0x{self._main_index:02X}/" - f"0x{self._sub_index:02X} is unavailable " - f"(error 0x{code:02X}): {description}" - ) - # 0x4F reports communication flags, not joint/mechanical faults. - # Its SI is an error-history slot, not the rejected object's SI. - raise ProtocolError( - f"O30 HOP communication error 0x{code:02X} at history slot " - f"0x{frame.sub_index:02X}: {description}" - ) - if frame.main_index != self._main_index or frame.rts != self._rts: - return False, None - if not frame.payload: - raise ProtocolError("O30 HOP response fragment is empty") - start = frame.sub_index - self._sub_index - end = start + len(frame.payload) - if start < 0 or end > self._length: - if end > 0 and start < self._length: - raise ProtocolError( - "O30 HOP response fragment overlaps requested range but exceeds it" - ) - return False, None - for offset, value in enumerate(frame.payload, start=start): - if self._received[offset] and self._buffer[offset] != value: - raise ProtocolError("O30 HOP duplicate fragment conflicts") - self._buffer[offset] = value - self._received[offset] = 1 - if not all(self._received): - return True, None - return True, bytes(self._buffer) +"""Compatibility exports for the shared HOP response implementation.""" +from ..hop.response import ( + HOPRangeAssembler, +) __all__ = ["HOPRangeAssembler"] diff --git a/src/linkerbot_hand/protocols/o30/sensors.py b/src/linkerbot_hand/protocols/o30/sensors.py index 70da6d9..34772d4 100644 --- a/src/linkerbot_hand/protocols/o30/sensors.py +++ b/src/linkerbot_hand/protocols/o30/sensors.py @@ -1,747 +1,12 @@ -"""Descriptor-driven O30 HOP 0.0.4 tactile sensor transactions.""" - -from __future__ import annotations - -import math -import struct -from collections.abc import Callable -from dataclasses import replace - -from ...api.enums import DataOrigin -from ...api.errors import ( - ProtocolError, - UnsupportedFirmwareError, - UnsupportedOperationError, -) -from ...api.readings import ( - TactileFieldDescriptor, - TactileFieldSample, - TactileSensorMetadata, - TactileSensorSample, -) -from ...capabilities.types import DeviceCapabilities, TelemetrySemantic -from ..base import ProtocolTransaction, ResponseProgress, SamplePayload -from .codec import ( - MI_SENSOR_DATA_1, - MI_SENSOR_DATA_2, - MI_SENSOR_DATA_3, - MI_SENSOR_INFO, -) -from .framing import ( - MAX_FRAGMENT_LENGTH, - HOPAddressing, - build_read_message, - build_write_message, -) -from .response import HOPRangeAssembler - -SENSOR_NAMES = ("thumb", "index", "middle", "ring", "pinky") - -_SI_SENSOR_TYPE = 0x00 -_SI_DATA_UNIT = 0x20 -_SI_DATA_RANGE = 0x28 -_SI_MAXIMUM_ROWS = 0x29 -_SI_MAXIMUM_COLUMNS = 0x2A -_SI_DESCRIPTOR_LENGTH = 0x2B -_SI_DESCRIPTOR = 0x2C -_SI_TOTAL_DATA_LENGTH = 0x6C -_SI_SELECTED_SENSOR = 0x6E -_SI_REQUESTED_ROWS = 0x6F -_SI_REQUESTED_COLUMNS = 0x70 -_SI_VALID_DATA_POSITION_MODE = 0x71 -_SI_RECTANGLE_FILL = 0x72 - -_METADATA_PREFIX_LENGTH = _SI_DESCRIPTOR - _SI_SENSOR_TYPE -_MAX_DESCRIPTOR_LENGTH = _SI_TOTAL_DATA_LENGTH - _SI_DESCRIPTOR -_SENSOR_CONTROL_LENGTH = 5 -_MAX_SENSOR_DATA_LENGTH = 765 -_STAGE_ESTIMATE_S = 0.003 -_RESPONSE_TIMEOUT_S = 0.1 -_LATE_RESPONSE_GUARD_S = 0.01 - -_SEMANTICS = frozenset( - {"Fn", "Ft", "Fd", "Px", "FnS", "FtS", "FdS", "Rsv", "x", "y", "z", "xyz"} +"""Compatibility exports for the shared HOP sensors implementation.""" + +from ..hop.sensors import ( + SENSOR_NAMES, + decode_tactile_fields, + parse_tactile_descriptor, + sensor_data_chunks, + tactile_transaction, ) -_DATA_TYPES = { - "U8": ("B", 1), - "I8": ("b", 1), - "U16": ("H", 2), - "I16": ("h", 2), - "U32": ("I", 4), - "I32": ("i", 4), - "F32": ("f", 4), -} - -_NextFactory = Callable[[], ProtocolTransaction | None] - - -def parse_tactile_descriptor( - descriptor_text: str, - *, - total_data_length: int | None = None, -) -> tuple[TactileFieldDescriptor, ...]: - """Parse one 0.0.4 descriptor and calculate every byte range.""" - - if not isinstance(descriptor_text, str) or not descriptor_text: - raise ProtocolError("O30 tactile descriptor must be a non-empty string") - if total_data_length is not None and ( - type(total_data_length) is not int or total_data_length < 0 - ): - raise ProtocolError("O30 tactile total data length must be non-negative") - - fields: list[TactileFieldDescriptor] = [] - byte_offset = 0 - for item in descriptor_text.split(";"): - parts = item.split("_") - if len(parts) != 3: - raise ProtocolError(f"O30 tactile descriptor field {item!r} is malformed") - semantic, data_type, count_text = parts - if semantic not in _SEMANTICS: - raise ProtocolError( - f"O30 tactile descriptor semantic {semantic!r} is unsupported" - ) - try: - _format, scalar_size = _DATA_TYPES[data_type] - except KeyError as error: - raise ProtocolError( - f"O30 tactile descriptor data type {data_type!r} is unsupported" - ) from error - if ( - not count_text.isascii() - or not count_text.isdecimal() - or count_text.startswith("0") - ): - raise ProtocolError( - f"O30 tactile descriptor count {count_text!r} is invalid" - ) - element_count = int(count_text) - scalar_count = element_count * (3 if semantic == "xyz" else 1) - byte_length = scalar_count * scalar_size - fields.append( - TactileFieldDescriptor( - semantic=semantic, - data_type=data_type, - element_count=element_count, - byte_offset=byte_offset, - byte_length=byte_length, - ) - ) - byte_offset += byte_length - - if total_data_length is not None and byte_offset != total_data_length: - raise ProtocolError( - "O30 tactile descriptor defines " - f"{byte_offset} bytes but device reports {total_data_length}" - ) - return tuple(fields) - - -def decode_tactile_fields( - descriptors: tuple[TactileFieldDescriptor, ...], - raw_data: bytes, -) -> tuple[TactileFieldSample, ...]: - """Decode exact little-endian values while preserving the source bytes.""" - - if not isinstance(descriptors, tuple) or not descriptors: - raise ProtocolError("O30 tactile field descriptors are empty") - if any(not isinstance(value, TactileFieldDescriptor) for value in descriptors): - raise ProtocolError("O30 tactile descriptors contain an invalid field") - if not isinstance(raw_data, bytes): - raise ProtocolError("O30 tactile data must be bytes") - expected_length = sum(value.byte_length for value in descriptors) - if len(raw_data) != expected_length: - raise ProtocolError( - f"O30 tactile data contains {len(raw_data)} bytes; " - f"descriptor requires {expected_length}" - ) - - samples: list[TactileFieldSample] = [] - for descriptor in descriptors: - format_code, _scalar_size = _DATA_TYPES[descriptor.data_type] - scalar_count = descriptor.element_count * ( - 3 if descriptor.semantic == "xyz" else 1 - ) - values = struct.unpack_from( - f"<{scalar_count}{format_code}", - raw_data, - descriptor.byte_offset, - ) - if any( - isinstance(value, float) and not math.isfinite(value) for value in values - ): - raise ProtocolError("O30 tactile F32 data must contain finite values") - decoded_values = ( - tuple( - (values[index], values[index + 1], values[index + 2]) - for index in range(0, len(values), 3) - ) - if descriptor.semantic == "xyz" - else tuple(values) - ) - samples.append( - TactileFieldSample( - semantic=descriptor.semantic, - data_type=descriptor.data_type, - element_count=descriptor.element_count, - byte_offset=descriptor.byte_offset, - byte_length=descriptor.byte_length, - values=decoded_values, - ) - ) - return tuple(samples) - - -def sensor_data_chunks(total_data_length: int) -> tuple[tuple[int, int, int], ...]: - """Return ``(main_index, sub_index, length)`` chunks for global sensor bytes.""" - - if type(total_data_length) is not int or not ( - 1 <= total_data_length <= _MAX_SENSOR_DATA_LENGTH - ): - raise ProtocolError( - f"O30 tactile data length must be in 1..{_MAX_SENSOR_DATA_LENGTH}" - ) - chunks: list[tuple[int, int, int]] = [] - global_offset = 0 - while global_offset < total_data_length: - if global_offset < 255: - main_index, channel_start, channel_end = MI_SENSOR_DATA_1, 0, 255 - elif global_offset < 510: - main_index, channel_start, channel_end = MI_SENSOR_DATA_2, 255, 510 - else: - main_index, channel_start, channel_end = MI_SENSOR_DATA_3, 510, 765 - length = min( - MAX_FRAGMENT_LENGTH, - total_data_length - global_offset, - channel_end - global_offset, - ) - chunks.append((main_index, global_offset - channel_start, length)) - global_offset += length - return tuple(chunks) - - -def tactile_transaction( - capabilities: DeviceCapabilities, - addressing: HOPAddressing, - *, - metadata_cache: dict[int, TactileSensorMetadata], -) -> ProtocolTransaction: - """Read all five O30 fingertip sensors as one atomic force sample.""" - - telemetry = capabilities.telemetry_channel(TelemetrySemantic.FORCE) - if telemetry is None or not telemetry.device_queried: - raise UnsupportedOperationError("O30 has no active tactile query") - if set(metadata_cache) == set(range(1, len(SENSOR_NAMES) + 1)): - return replace( - _data_transaction(metadata_cache, addressing=addressing), - firmware_optional=True, - ) - - metadata: dict[int, TactileSensorMetadata] = {} - first = _metadata_select_transaction( - 1, - metadata, - addressing=addressing, - metadata_cache=metadata_cache, - ) - return replace(first, firmware_optional=True) - - -def _metadata_select_transaction( - selected_sensor: int, - metadata: dict[int, TactileSensorMetadata], - *, - addressing: HOPAddressing, - metadata_cache: dict[int, TactileSensorMetadata], -) -> ProtocolTransaction: - payload = bytes((selected_sensor,)) - - def next_stage() -> ProtocolTransaction: - return _metadata_prefix_transaction( - selected_sensor, - metadata, - addressing=addressing, - metadata_cache=metadata_cache, - ) - - return _write_stage( - key=("read", TelemetrySemantic.FORCE, "select_metadata", selected_sensor), - sub_index=_SI_SELECTED_SENSOR, - payload=payload, - addressing=addressing, - next_factory=next_stage, - ) - - -def _metadata_prefix_transaction( - selected_sensor: int, - metadata: dict[int, TactileSensorMetadata], - *, - addressing: HOPAddressing, - metadata_cache: dict[int, TactileSensorMetadata], -) -> ProtocolTransaction: - values: dict[str, bytes] = {} - - def consume_prefix(data: bytes) -> None: - values["prefix"] = data - descriptor_length = data[_SI_DESCRIPTOR_LENGTH] - if descriptor_length > _MAX_DESCRIPTOR_LENGTH: - raise ProtocolError( - f"O30 tactile descriptor length {descriptor_length} exceeds 64" - ) - - def next_stage() -> ProtocolTransaction: - descriptor_length = values["prefix"][_SI_DESCRIPTOR_LENGTH] - if descriptor_length == 0: - values["descriptor"] = b"" - return _metadata_total_length_transaction( - selected_sensor, - metadata, - values, - addressing=addressing, - metadata_cache=metadata_cache, - ) - return _metadata_descriptor_transaction( - selected_sensor, - metadata, - values, - descriptor_length=descriptor_length, - addressing=addressing, - metadata_cache=metadata_cache, - ) - - return _read_stage( - key=("read", TelemetrySemantic.FORCE, "metadata_prefix", selected_sensor), - main_index=MI_SENSOR_INFO, - sub_index=_SI_SENSOR_TYPE, - length=_METADATA_PREFIX_LENGTH, - on_data=consume_prefix, - addressing=addressing, - next_factory=next_stage, - ) - - -def _metadata_descriptor_transaction( - selected_sensor: int, - metadata: dict[int, TactileSensorMetadata], - values: dict[str, bytes], - *, - descriptor_length: int, - addressing: HOPAddressing, - metadata_cache: dict[int, TactileSensorMetadata], -) -> ProtocolTransaction: - def consume_descriptor(data: bytes) -> None: - values["descriptor"] = data - - def next_stage() -> ProtocolTransaction: - return _metadata_total_length_transaction( - selected_sensor, - metadata, - values, - addressing=addressing, - metadata_cache=metadata_cache, - ) - - return _read_stage( - key=("read", TelemetrySemantic.FORCE, "descriptor", selected_sensor), - main_index=MI_SENSOR_INFO, - sub_index=_SI_DESCRIPTOR, - length=descriptor_length, - on_data=consume_descriptor, - addressing=addressing, - next_factory=next_stage, - ) - - -def _metadata_total_length_transaction( - selected_sensor: int, - metadata: dict[int, TactileSensorMetadata], - values: dict[str, bytes], - *, - addressing: HOPAddressing, - metadata_cache: dict[int, TactileSensorMetadata], -) -> ProtocolTransaction: - def consume_total_length(data: bytes) -> None: - values["total_data_length"] = data - - def next_stage() -> ProtocolTransaction: - return _metadata_controls_transaction( - selected_sensor, - metadata, - values, - addressing=addressing, - metadata_cache=metadata_cache, - ) - - return _read_stage( - key=("read", TelemetrySemantic.FORCE, "total_length", selected_sensor), - main_index=MI_SENSOR_INFO, - sub_index=_SI_TOTAL_DATA_LENGTH, - length=2, - on_data=consume_total_length, - addressing=addressing, - next_factory=next_stage, - ) - - -def _metadata_controls_transaction( - selected_sensor: int, - metadata: dict[int, TactileSensorMetadata], - values: dict[str, bytes], - *, - addressing: HOPAddressing, - metadata_cache: dict[int, TactileSensorMetadata], -) -> ProtocolTransaction: - final_sensor = selected_sensor == len(SENSOR_NAMES) - - def consume_controls(data: bytes) -> None: - parsed = _build_metadata(selected_sensor, values, data) - metadata[selected_sensor] = parsed - if final_sensor: - metadata_cache.clear() - metadata_cache.update(metadata) - - def next_stage() -> ProtocolTransaction: - if not final_sensor: - return _metadata_select_transaction( - selected_sensor + 1, - metadata, - addressing=addressing, - metadata_cache=metadata_cache, - ) - return _data_transaction(metadata, addressing=addressing) - - return _read_stage( - key=("read", TelemetrySemantic.FORCE, "controls", selected_sensor), - main_index=MI_SENSOR_INFO, - sub_index=_SI_SELECTED_SENSOR, - length=_SENSOR_CONTROL_LENGTH, - on_data=consume_controls, - addressing=addressing, - next_factory=next_stage, - ) - - -def _build_metadata( - selected_sensor: int, - values: dict[str, bytes], - controls: bytes, -) -> TactileSensorMetadata: - prefix = values["prefix"] - sensor_type = _decode_padded_text(prefix[:_SI_DATA_UNIT], "sensor type") - if sensor_type is None: - raise ProtocolError("O30 tactile sensor type is empty") - data_unit = _decode_padded_text( - prefix[_SI_DATA_UNIT:_SI_DATA_RANGE], - "data unit", - ) - descriptor_data = values["descriptor"] - descriptor_text = _decode_descriptor(descriptor_data) - total_data_length = int.from_bytes(values["total_data_length"], "little") - if controls[0] != selected_sensor: - raise ProtocolError( - f"O30 selected sensor changed from {selected_sensor} to {controls[0]}" - ) - valid_data_position_mode = _decode_switch( - controls[_SI_VALID_DATA_POSITION_MODE - _SI_SELECTED_SENSOR], - "valid data position mode", - ) - rectangle_fill = _decode_switch( - controls[_SI_RECTANGLE_FILL - _SI_SELECTED_SENSOR], - "rectangle fill", - ) - - if descriptor_text is None: - if total_data_length != 0: - raise ProtocolError("O30 tactile sensor reports data without a descriptor") - if sensor_type != "NO_SENSOR": - raise ProtocolError("O30 installed tactile sensor has no descriptor") - descriptors: tuple[TactileFieldDescriptor, ...] = () - else: - if sensor_type == "NO_SENSOR": - raise ProtocolError("O30 NO_SENSOR metadata contains a descriptor") - descriptors = parse_tactile_descriptor( - descriptor_text, - total_data_length=total_data_length, - ) - if total_data_length > _MAX_SENSOR_DATA_LENGTH: - raise ProtocolError("O30 tactile data exceeds three 255-byte pages") - return TactileSensorMetadata( - selected_sensor=selected_sensor, - sensor_name=SENSOR_NAMES[selected_sensor - 1], - sensor_type=sensor_type, - data_unit=data_unit, - data_range=prefix[_SI_DATA_RANGE], - maximum_rows=prefix[_SI_MAXIMUM_ROWS], - maximum_columns=prefix[_SI_MAXIMUM_COLUMNS], - requested_rows=controls[_SI_REQUESTED_ROWS - _SI_SELECTED_SENSOR], - requested_columns=controls[_SI_REQUESTED_COLUMNS - _SI_SELECTED_SENSOR], - valid_data_position_mode=valid_data_position_mode, - rectangle_fill=rectangle_fill, - descriptor_text=descriptor_text, - field_descriptors=descriptors, - total_data_length=total_data_length, - ) - - -def _data_transaction( - metadata: dict[int, TactileSensorMetadata], - *, - addressing: HOPAddressing, -) -> ProtocolTransaction: - available = tuple( - selected_sensor - for selected_sensor in range(1, len(SENSOR_NAMES) + 1) - if metadata[selected_sensor].available - ) - if not available: - raise UnsupportedFirmwareError("O30 reports no available fingertip sensors") - # Keep five stable identities. An absent sensor has no bytes or fields; - # it is distinct from a present sensor that measured zero force. - samples = { - index: TactileSensorSample(metadata=value, raw_data=b"", fields=()) - for index, value in metadata.items() - if not value.available - } - return _data_select_transaction( - available, - 0, - metadata, - samples, - addressing=addressing, - ) - - -def _data_select_transaction( - available: tuple[int, ...], - sensor_position: int, - metadata: dict[int, TactileSensorMetadata], - samples: dict[int, TactileSensorSample], - *, - addressing: HOPAddressing, -) -> ProtocolTransaction: - selected_sensor = available[sensor_position] - payload = bytes((selected_sensor,)) - - def next_stage() -> ProtocolTransaction: - return _data_chunk_transaction( - available, - sensor_position, - metadata, - samples, - chunks=sensor_data_chunks(metadata[selected_sensor].total_data_length), - chunk_position=0, - raw_data=bytearray(metadata[selected_sensor].total_data_length), - global_offset=0, - addressing=addressing, - ) - - return _write_stage( - key=("read", TelemetrySemantic.FORCE, "select_data", selected_sensor), - sub_index=_SI_SELECTED_SENSOR, - payload=payload, - addressing=addressing, - next_factory=next_stage, - ) - - -def _data_chunk_transaction( - available: tuple[int, ...], - sensor_position: int, - metadata: dict[int, TactileSensorMetadata], - samples: dict[int, TactileSensorSample], - *, - chunks: tuple[tuple[int, int, int], ...], - chunk_position: int, - raw_data: bytearray, - global_offset: int, - addressing: HOPAddressing, -) -> ProtocolTransaction: - selected_sensor = available[sensor_position] - main_index, sub_index, length = chunks[chunk_position] - final_chunk = chunk_position + 1 == len(chunks) - final_sensor = sensor_position + 1 == len(available) - - def consume_chunk(data: bytes) -> SamplePayload | None: - raw_data[global_offset : global_offset + length] = data - if not final_chunk: - return None - sensor_metadata = metadata[selected_sensor] - exact_data = bytes(raw_data) - samples[selected_sensor] = TactileSensorSample( - metadata=sensor_metadata, - raw_data=exact_data, - fields=decode_tactile_fields( - sensor_metadata.field_descriptors, - exact_data, - ), - ) - if not final_sensor: - return None - return SamplePayload( - semantic=TelemetrySemantic.FORCE, - sensors=tuple(samples[index] for index in range(1, len(SENSOR_NAMES) + 1)), - origin=DataOrigin.DEVICE_RESPONSE, - ) - - def next_stage() -> ProtocolTransaction: - if not final_chunk: - return _data_chunk_transaction( - available, - sensor_position, - metadata, - samples, - chunks=chunks, - chunk_position=chunk_position + 1, - raw_data=raw_data, - global_offset=global_offset + length, - addressing=addressing, - ) - return _data_select_transaction( - available, - sensor_position + 1, - metadata, - samples, - addressing=addressing, - ) - - has_next = not (final_chunk and final_sensor) - return _read_stage( - key=( - "read", - TelemetrySemantic.FORCE, - "data", - selected_sensor, - chunk_position, - ), - main_index=main_index, - sub_index=sub_index, - length=length, - on_data=consume_chunk, - addressing=addressing, - next_factory=next_stage if has_next else None, - ) - - -def _read_stage( - *, - key: object, - main_index: int, - sub_index: int, - length: int, - on_data: Callable[[bytes], SamplePayload | None] | Callable[[bytes], None], - addressing: HOPAddressing, - next_factory: _NextFactory | None, -) -> ProtocolTransaction: - assembler = HOPRangeAssembler( - main_index, - sub_index=sub_index, - length=length, - rts=False, - response_id=addressing.response_id, - object_unavailable_is_unsupported=True, - ) - - def consume(message: object) -> ResponseProgress: - matched, complete = assembler.consume(message) - if complete is None: - return ResponseProgress(matched=matched) - result = on_data(complete) - return ResponseProgress( - matched=True, - complete=True, - result=result, - ) - - return ProtocolTransaction( - key=key, - correlation_key=("o30_hop", addressing.response_id), - request_messages=( - build_read_message( - main_index, - sub_index=sub_index, - length=length, - request_id=addressing.request_id, - ), - ), - response_timeout_s=_RESPONSE_TIMEOUT_S, - estimated_duration_s=_STAGE_ESTIMATE_S, - response_consumer=consume, - late_response_guard_s=_LATE_RESPONSE_GUARD_S, - next_transaction_factory=next_factory, - yield_after_response=next_factory is not None, - ) - - -def _write_stage( - *, - key: object, - sub_index: int, - payload: bytes, - addressing: HOPAddressing, - next_factory: _NextFactory, -) -> ProtocolTransaction: - assembler = HOPRangeAssembler( - MI_SENSOR_INFO, - sub_index=sub_index, - length=len(payload), - rts=False, - response_id=addressing.response_id, - object_unavailable_is_unsupported=True, - ) - - def consume(message: object) -> ResponseProgress: - matched, complete = assembler.consume(message) - if complete is None: - return ResponseProgress(matched=matched) - if complete != payload: - raise ProtocolError("O30 tactile configuration response did not echo value") - return ResponseProgress(matched=True, complete=True) - - return ProtocolTransaction( - key=key, - correlation_key=("o30_hop", addressing.response_id), - request_messages=( - build_write_message( - MI_SENSOR_INFO, - sub_index=sub_index, - payload=payload, - request_id=addressing.request_id, - ), - ), - response_timeout_s=_RESPONSE_TIMEOUT_S, - estimated_duration_s=_STAGE_ESTIMATE_S, - response_consumer=consume, - late_response_guard_s=_LATE_RESPONSE_GUARD_S, - next_transaction_factory=next_factory, - yield_after_response=True, - ) - - -def _decode_padded_text(data: bytes, label: str) -> str | None: - try: - value = data.split(b"\x00", 1)[0].decode("utf-8").strip() - except UnicodeDecodeError as error: - raise ProtocolError(f"O30 tactile {label} is not UTF-8") from error - return value or None - - -def _decode_descriptor(data: bytes) -> str | None: - if not data: - return None - try: - value = data.decode("ascii") - except UnicodeDecodeError as error: - raise ProtocolError("O30 tactile descriptor is not ASCII") from error - if not value or "\x00" in value or value.strip() != value: - raise ProtocolError("O30 tactile descriptor contains invalid padding") - return value - - -def _decode_switch(value: int, label: str) -> bool: - if value not in {0, 1}: - raise ProtocolError(f"O30 tactile {label} must be zero or one") - return bool(value) - __all__ = [ "SENSOR_NAMES", diff --git a/src/linkerbot_hand/protocols/o30/telemetry.py b/src/linkerbot_hand/protocols/o30/telemetry.py index c875dbf..21bb215 100644 --- a/src/linkerbot_hand/protocols/o30/telemetry.py +++ b/src/linkerbot_hand/protocols/o30/telemetry.py @@ -10,6 +10,7 @@ from ...api.readings import JointFaultStatus from ...capabilities.types import DeviceCapabilities, TelemetrySemantic from ..base import ProtocolTransaction, ResponseProgress, SamplePayload +from ..hop.response import serialize_classic_read from .codec import ( MI_ACCELERATION, MI_CURRENT, @@ -152,9 +153,9 @@ def consume(message: object) -> ResponseProgress: ), ) - return ProtocolTransaction( + transaction = ProtocolTransaction( key=("read", semantic), - correlation_key=("o30_hop", addressing.response_id), + correlation_key=("hop", addressing.response_id), request_messages=( build_read_message( profile.main_index, @@ -162,6 +163,8 @@ def consume(message: object) -> ResponseProgress: length=WIRE_SLOT_COUNT, rts=profile.rts, request_id=addressing.request_id, + brs=addressing.brs, + can_fd=addressing.can_fd, ), ), response_timeout_s=0.1, @@ -169,6 +172,7 @@ def consume(message: object) -> ResponseProgress: response_consumer=consume, late_response_guard_s=0.01, ) + return serialize_classic_read(transaction, addressing) def _within_declared_range( diff --git a/src/linkerbot_hand/protocols/o30/transactions.py b/src/linkerbot_hand/protocols/o30/transactions.py index af83fd1..40ede02 100644 --- a/src/linkerbot_hand/protocols/o30/transactions.py +++ b/src/linkerbot_hand/protocols/o30/transactions.py @@ -1,148 +1,55 @@ -"""O30 protocol backend for the unified device runtime.""" - -from __future__ import annotations - -from linkerbot_transport import CANRouteKey +"""O30 installed V25 objects atop the shared HOP endpoint mechanics.""" from ...api.enums import HandSide -from ...api.errors import ProtocolError, UnsupportedOperationError, ValidationError -from ...api.readings import RawNumber, TactileSensorMetadata -from ...capabilities.types import ( - CommandSemantic, - DeviceCapabilities, - TelemetrySemantic, -) -from ..base import IdentityResult, ProtocolTransaction, SamplePayload +from ...api.errors import UnsupportedOperationError +from ...api.readings import RawNumber +from ...capabilities.types import CommandSemantic, DeviceCapabilities, TelemetrySemantic +from ..base import ProtocolTransaction +from ..hop.backend import HOPBackend from .control import command_transaction -from .framing import HOPAddressing, addressing_for_side, response_route -from .identity import o30_identity_transaction -from .sensors import tactile_transaction +from .framing import addressing_for_side from .telemetry import read_transaction -_BASELINE = ( - TelemetrySemantic.ANGLES, - TelemetrySemantic.FAULTS, - TelemetrySemantic.STALL_DETECTION_TIMES, - TelemetrySemantic.STALL_THRESHOLDS, - TelemetrySemantic.STALL_HOLD_CURRENTS, -) - - -class O30ProtocolBackend: - """O30 standard HOP endpoint; default request IDs are right 1, left 2.""" - - def __init__(self, side: HandSide, *, request_id: int | None = None) -> None: - if side not in {HandSide.LEFT, HandSide.RIGHT}: - raise ProtocolError("O30 protocol requires a left or right side") - self._side = side - if request_id is not None and type(request_id) is not int: - raise ValidationError("O30 request_id must be int or None") - self._addressing = ( - addressing_for_side(side) - if request_id is None - else HOPAddressing(request_id, request_id | 0x400) - ) - self._endpoint_id = ( - "o30", - self._addressing.request_id, - self._addressing.response_id, - ) - self._route_keys = (response_route(self._addressing.response_id),) - self._sensor_metadata: dict[int, TactileSensorMetadata] = {} - self._identity_fingerprint: object | None = None - - @property - def endpoint_id(self) -> tuple[str, int, int]: - """Return the physical HOP endpoint used for TX arbitration.""" - - return self._endpoint_id - @property - def route_keys(self) -> tuple[CANRouteKey, ...]: - """Return the exact standard response-ID route.""" +class O30ProtocolBackend(HOPBackend): + """Preserve deployed O30 defaults: right ID=1, left ID=2, BRS off.""" - return self._route_keys - - def identity_transaction( + def __init__( self, - capabilities: DeviceCapabilities, - ) -> ProtocolTransaction: - """Build the response-gated field-wise identity read.""" - - if capabilities.side is not self._side: - raise ProtocolError("O30 capability side changed after backend creation") - # Initialization always supplies the unverified declared profile; - # same-generation get_device_info() supplies the verified profile. - if not capabilities.identity.verified: - self._sensor_metadata.clear() - self._identity_fingerprint = None - return o30_identity_transaction( - capabilities, self._addressing, on_identity=self._observe_identity + side: HandSide, + *, + request_id: int | None = None, + brs: bool = False, + can_fd: bool = True, + ) -> None: + super().__init__( + side, + request_id=addressing_for_side(side).request_id + if request_id is None + else request_id, + brs=brs, + can_fd=can_fd, ) - def _observe_identity(self, identity: IdentityResult) -> None: - if identity.fingerprint != self._identity_fingerprint: - self._sensor_metadata.clear() - self._identity_fingerprint = identity.fingerprint - - def baseline_semantics( - self, - capabilities: DeviceCapabilities, - ) -> tuple[TelemetrySemantic, ...]: - """Return motion, fault, and safety configuration required before ready.""" - - for semantic in _BASELINE: - telemetry = capabilities.telemetry_channel(semantic) - if telemetry is None or not telemetry.device_queried: - raise ProtocolError( - f"O30 baseline semantic {semantic.value} is not queryable" - ) - return _BASELINE - def command_transaction( self, semantic: CommandSemantic, values: tuple[RawNumber, ...], capabilities: DeviceCapabilities, ) -> ProtocolTransaction: - """Build one V25 write completed only by its HOP response.""" - return command_transaction(semantic, values, capabilities, self._addressing) def read_transaction( - self, - semantic: TelemetrySemantic, - capabilities: DeviceCapabilities, + self, semantic: TelemetrySemantic, capabilities: DeviceCapabilities ) -> ProtocolTransaction: - """Build one V25 actual-state or set-point query.""" - if semantic is TelemetrySemantic.FORCE: - return tactile_transaction( - capabilities, - self._addressing, - metadata_cache=self._sensor_metadata, - ) + return self.sensor_transaction(capabilities) return read_transaction(semantic, capabilities, self._addressing) def operation_transaction( - self, - operation: str, - capabilities: DeviceCapabilities, + self, operation: str, capabilities: DeviceCapabilities ) -> ProtocolTransaction: - """Reject operations not represented by a verified O30 object.""" - - del capabilities raise UnsupportedOperationError(f"O30 does not support {operation}") - def decode_report( - self, - message: object, - capabilities: DeviceCapabilities, - ) -> tuple[SamplePayload, ...]: - """Ignore unsolicited traffic; the profile uses explicit HOP queries.""" - - del message, capabilities - return () - __all__ = ["O30ProtocolBackend"] diff --git a/src/linkerbot_hand/protocols/registry.py b/src/linkerbot_hand/protocols/registry.py index b7362a2..614d38d 100644 --- a/src/linkerbot_hand/protocols/registry.py +++ b/src/linkerbot_hand/protocols/registry.py @@ -4,7 +4,7 @@ from collections.abc import Callable -from ..api.enums import HandSide, TransportKind +from ..api.enums import HandSide, ProtocolKind, TransportKind from ..api.errors import UnsupportedOperationError, ValidationError from ..capabilities.types import DeviceCapabilities from .base import ProtocolBackend @@ -40,7 +40,11 @@ def create_protocol_backend( - capabilities: DeviceCapabilities, *, o30_request_id: int | None = None + capabilities: DeviceCapabilities, + *, + o30_request_id: int | None = None, + hop_request_id: int | None = None, + hop_brs: bool = False, ) -> ProtocolBackend: """Create the stateless backend declared by one capability profile. @@ -65,8 +69,28 @@ def create_protocol_backend( operation="open", model=capabilities.model, ) from error + if capabilities.protocol is ProtocolKind.HOP and capabilities.model in {"L6", "O6"}: + from .hop.six_axis import HOPSixAxisBackend + + if o30_request_id is not None: + raise ValidationError("o30_request_id is only valid for O30") + if capabilities.transport.kind is not TransportKind.CAN_FD and not ( + capabilities.model == "O6" + and capabilities.transport.kind is TransportKind.CAN + ): + raise ValidationError("HOP requires a supported CAN or CAN FD profile") + return HOPSixAxisBackend( + capabilities.model, + capabilities.side, + request_id=hop_request_id, + brs=hop_brs, + can_fd=capabilities.transport.kind is TransportKind.CAN_FD, + ) expected_transport = _TRANSPORT_KINDS[capabilities.model] - if capabilities.transport.kind is not expected_transport: + classic_o30 = ( + capabilities.model == "O30" and capabilities.transport.kind is TransportKind.CAN + ) + if capabilities.transport.kind is not expected_transport and not classic_o30: raise ValidationError( f"{capabilities.model} protocol backend requires " f"{expected_transport.value} capabilities" @@ -80,10 +104,20 @@ def create_protocol_backend( raise ValidationError( f"{capabilities.model} protocol backend has an invalid side" ) - if o30_request_id is not None: + if hop_request_id is not None or hop_brs: if capabilities.model != "O30": - raise ValidationError("o30_request_id is only valid for O30") - return O30ProtocolBackend(capabilities.side, request_id=o30_request_id) + raise ValidationError("HOP configuration requires a HOP backend") + if o30_request_id is not None and hop_request_id not in (None, o30_request_id): + raise ValidationError("conflicting HOP request IDs") + if capabilities.model == "O30": + return O30ProtocolBackend( + capabilities.side, + request_id=hop_request_id if hop_request_id is not None else o30_request_id, + brs=hop_brs, + can_fd=not classic_o30, + ) + if o30_request_id is not None: + raise ValidationError("o30_request_id is only valid for O30") return factory(capabilities.side) diff --git a/src/linkerbot_hand/runtime/_can_configuration.py b/src/linkerbot_hand/runtime/_can_configuration.py new file mode 100644 index 0000000..d02d2b2 --- /dev/null +++ b/src/linkerbot_hand/runtime/_can_configuration.py @@ -0,0 +1,22 @@ +"""Private actor request keys for optional device-side CAN management.""" + +from dataclasses import dataclass +from typing import Protocol + +from ..protocols.base import ProtocolTransaction + +CAN_CONFIGURATION_KEY = ("can_configuration",) + + +@dataclass(frozen=True, slots=True) +class CANConfigurationWrite: + sub_index: int + payload: bytes + deadline_s: float + + +class CANConfigurationBackend(Protocol): + def can_configuration_transaction(self) -> ProtocolTransaction: ... + def can_configuration_write( + self, sub_index: int, payload: bytes + ) -> ProtocolTransaction: ... diff --git a/src/linkerbot_hand/runtime/_device_actor.py b/src/linkerbot_hand/runtime/_device_actor.py index 6e6dd22..7056da2 100644 --- a/src/linkerbot_hand/runtime/_device_actor.py +++ b/src/linkerbot_hand/runtime/_device_actor.py @@ -6,7 +6,9 @@ import threading from collections.abc import Callable from dataclasses import dataclass +from typing import cast +from ..api.can_configuration import CANConfiguration from ..api.commands import AckSupport, CommandFamily, CommandStage from ..api.config import DeviceConfig, PollingPlan from ..api.enums import DataOrigin @@ -24,6 +26,11 @@ from ..capabilities.force import FORCE_QUERIES from ..capabilities.types import DeviceCapabilities, TelemetrySemantic from ..protocols.base import ProtocolBackend, ProtocolTransaction +from ._can_configuration import ( + CAN_CONFIGURATION_KEY, + CANConfigurationBackend, + CANConfigurationWrite, +) from ._clock import RuntimeClock from ._connection_initializer import ConnectionInitializer from ._errors import ResponseTimeoutError, as_hand_error @@ -107,6 +114,7 @@ def __init__( clock=clock, identity_timeout_s=config.identity_timeout_s, ) + self._can_reconfiguring = threading.Event() self._control_lock = threading.Lock() self._initial_ticket: OperationTicket | None = None self._recovery = _RecoveryState(delay_s=config.reconnect_initial_delay_s) @@ -128,6 +136,7 @@ def start(self) -> OperationTicket: raise ProtocolError("actor has no initialization ticket") return self._initial_ticket self._stop = threading.Event() + self._can_reconfiguring.clear() self._mailbox.reset() self._initial_ticket = OperationTicket( deadline_at_s=self._clock.monotonic() + self._config.shutdown_timeout_s @@ -150,6 +159,10 @@ def start(self) -> OperationTicket: self._thread.start() return self._initial_ticket + def begin_can_reconfiguration(self) -> None: + """Suppress automatic recovery with old connection parameters.""" + self._can_reconfiguring.set() + def request_recovery(self, *, preparation_deadline_s: float) -> OperationTicket: """Coalesce explicit recovery callers onto the next safe generation.""" @@ -308,7 +321,7 @@ def _main_loop(self, runner: TransactionRunner) -> None: if receiver_error is not None and self._state.ready(): self._disconnect(self._classify(receiver_error, "receive")) continue - if self._auto_recovery_due(): + if not self._can_reconfiguring.is_set() and self._auto_recovery_due(): runner = self._perform_recovery(runner) continue if not self._state.ready(): @@ -556,6 +569,38 @@ def _execute_read(self, runner: TransactionRunner, claim: TransactionClaim) -> N poll_in_flight = False transaction: ProtocolTransaction | None = None try: + if claim.key == CAN_CONFIGURATION_KEY or isinstance( + claim.key, CANConfigurationWrite + ): + backend = cast(CANConfigurationBackend, self._backend) + changing = isinstance(claim.key, CANConfigurationWrite) + transaction = ( + backend.can_configuration_write( + claim.key.sub_index, claim.key.payload + ) + if isinstance(claim.key, CANConfigurationWrite) + else backend.can_configuration_transaction() + ) + configuration_runner = ( + self._make_runner(execution_deadline_at_s=claim.key.deadline_s) + if isinstance(claim.key, CANConfigurationWrite) + else runner + ) + outcome = configuration_runner.execute( + transaction, + capabilities=self._state.capabilities(), + publish_generation=self._state.generation(), + latest_family=None, + priority=80, + ) + if changing: + self._state.mark_not_ready() + self._transactions.complete(claim.key, True, sequence=1) + else: + if not isinstance(outcome.result, CANConfiguration): + raise ProtocolError("invalid CAN configuration response") + self._transactions.complete(claim.key, outcome.result, sequence=1) + return if claim.key == DEVICE_INFO_KEY: published, sequence = self._initializer.refresh_identity(runner) self._transactions.complete( @@ -1048,7 +1093,7 @@ def _auto_recovery_due(self) -> bool: ) def _recovery_deadline(self) -> float | None: - if not self._config.auto_reconnect: + if not self._config.auto_reconnect or self._can_reconfiguring.is_set(): return None with self._control_lock: return None if self._performing_recovery else self._recovery.due_at_s diff --git a/src/linkerbot_hand/runtime/_runtime_state.py b/src/linkerbot_hand/runtime/_runtime_state.py index ac8f5a1..df4ee75 100644 --- a/src/linkerbot_hand/runtime/_runtime_state.py +++ b/src/linkerbot_hand/runtime/_runtime_state.py @@ -102,6 +102,13 @@ def validate_identity(self, identity: IdentityResult, *, recovery: bool) -> None model=self._declared.model, generation=self.generation(), ) + if capabilities.protocol is not self._declared.protocol: + raise IdentityMismatchError( + "connected device protocol does not match configuration", + operation="open" if not recovery else "recover", + model=self._declared.model, + generation=self.generation(), + ) if capabilities.side is not self._declared.side: raise IdentityMismatchError( "connected device side does not match configuration", diff --git a/src/linkerbot_hand/runtime/_transaction_runner.py b/src/linkerbot_hand/runtime/_transaction_runner.py index 34b301c..b617f06 100644 --- a/src/linkerbot_hand/runtime/_transaction_runner.py +++ b/src/linkerbot_hand/runtime/_transaction_runner.py @@ -8,6 +8,7 @@ from linkerbot_transport import SendOutcome, SendReceipt +from ..api.can_configuration import CANConfiguration from ..api.errors import ( ClosedError, DeviceNotReadyError, @@ -46,7 +47,7 @@ class DispatchGuard: class TransactionOutcome: """Decoded result and publications from one complete transaction.""" - result: IdentityResult | SamplePayload | None + result: IdentityResult | SamplePayload | CANConfiguration | None acknowledged: bool publications: tuple[tuple[SamplePayload, object], ...] sent_frames: int @@ -56,7 +57,7 @@ class TransactionOutcome: class _StageOutcome: """Internal result of one dispatched stage in a transaction chain.""" - result: IdentityResult | SamplePayload | None + result: IdentityResult | SamplePayload | CANConfiguration | None acknowledged: bool publications: tuple[tuple[SamplePayload, object], ...] sent_frames: int @@ -279,7 +280,7 @@ def complete( started_at_s = pending.started_at_s sent_frames = 0 publications: tuple[tuple[SamplePayload, object], ...] = () - result: IdentityResult | SamplePayload | None = None + result: IdentityResult | SamplePayload | CANConfiguration | None = None acknowledged = False current = pending diff --git a/src/linkerbot_hand/runtime/device_runtime.py b/src/linkerbot_hand/runtime/device_runtime.py index ee10cfc..9c43bb3 100644 --- a/src/linkerbot_hand/runtime/device_runtime.py +++ b/src/linkerbot_hand/runtime/device_runtime.py @@ -6,6 +6,7 @@ from collections.abc import Hashable from typing import cast +from ..api.can_configuration import CANConfiguration from ..api.commands import AckSupport, CommandHandle from ..api.config import DeviceConfig, PollingPlan from ..api.device_info import DeviceInfo @@ -28,6 +29,11 @@ ) from ..protocols.base import ProtocolBackend from ..protocols.registry import create_protocol_backend +from ._can_configuration import ( + CAN_CONFIGURATION_KEY, + CANConfigurationBackend, + CANConfigurationWrite, +) from ._clock import resolve_clock from ._device_actor import DeviceActor from ._errors import as_hand_error @@ -325,6 +331,61 @@ def read_device_info(self, timeout: float) -> DeviceInfo: raise LifecycleError("identity transaction returned an invalid type") return result + def read_can_configuration(self, timeout: float) -> CANConfiguration: + self._require_ready("get_can_configuration") + if not self._state.capabilities().operations.read_can_configuration: + raise UnsupportedOperationError("CAN configuration is unsupported") + waiter = self._transactions.request( + CAN_CONFIGURATION_KEY, + start_sequence=0, + deadline_s=self._clock.monotonic() + timeout, + ) + result = waiter.wait() + if not isinstance(result, CANConfiguration): + raise LifecycleError( + "CAN configuration transaction returned an invalid type" + ) + return result + + def write_can_configuration( + self, sub_index: int, payload: bytes, timeout: float + ) -> None: + # Same lock as start_polling/stop_polling/recover/close: the test and + # exclusive write admission form a single lifecycle transition. + with self._operation_lock: + self._require_ready("set_can_configuration") + if not self._state.capabilities().operations.write_can_configuration: + raise UnsupportedOperationError("CAN configuration is unsupported") + if self._polling.plan is not None: + raise LifecycleError( + "stop_polling() before changing CAN ID, BRS or bitrates", + operation="set_can_configuration", + ) + backend = cast(CANConfigurationBackend, self._backend) + backend.can_configuration_write( + sub_index, payload + ) # Validate before admission changes. + key = CANConfigurationWrite( + sub_index, payload, self._clock.monotonic() + timeout + ) + error = DeviceNotReadyError( + "CAN settings are changing; reconnect with the new configuration" + ) + try: + self._state.stop_admission() + self._actor.begin_can_reconfiguration() + self._commands.reject_new(error) + self._commands.fail_pending(error) + waiter = self._transactions.request_exclusive( + key, + start_sequence=0, + deadline_s=key.deadline_s, + error=error, + ) + waiter.wait() + finally: + self.close() + def operate(self, operation: str) -> CommandHandle: """Accept one capability-checked discrete operation.""" @@ -473,7 +534,10 @@ def _resolve_backend( ) -> ProtocolBackend: if backend is None: backend = create_protocol_backend( - capabilities, o30_request_id=config.o30_request_id + capabilities, + o30_request_id=config.o30_request_id, + hop_request_id=config.hop_request_id, + hop_brs=config.hop_brs, ) for name in ( "identity_transaction", diff --git a/src/linkerbot_hand/runtime/transactions.py b/src/linkerbot_hand/runtime/transactions.py index 010edb7..3413830 100644 --- a/src/linkerbot_hand/runtime/transactions.py +++ b/src/linkerbot_hand/runtime/transactions.py @@ -172,6 +172,51 @@ def request( self._wake() return waiter + def request_exclusive( + self, + key: Hashable, + *, + start_sequence: int, + deadline_s: float, + error: HandError, + ) -> TransactionWaiter: + """Atomically replace read demand and close ordinary admission. + + An actor-owned transaction may still finish, but unclaimed work is + discarded. Notify old callers after releasing the coordinator lock. + """ + + _validate_key(key) + if type(start_sequence) is not int or start_sequence < 0: + raise ValidationError("start_sequence must be a non-negative int") + if not isinstance(error, HandError): + raise ValidationError("error must be HandError") + with self._lock: + if not self._accepting: + raise self._admission_error + if deadline_s <= self._clock.monotonic(): + raise TimeoutError("exclusive request deadline has already expired") + # Prepare the replacement before changing admission or old work. + waiter = TransactionWaiter( + key=key, + start_sequence=start_sequence, + deadline_s=deadline_s, + clock=self._clock, + cancel=self.cancel, + ) + old_waiters = tuple( + old for entry in self._entries.values() for old in entry.waiters + ) + self._accepting = False + self._admission_error = error + self._entries.clear() + self._entry_locked(key).waiters.append(waiter) + self._waiter_count = 1 + for old in old_waiters: + old.fail(error) + self._wake() + return waiter + def request_poll(self, key: Hashable) -> bool: """Create or join one periodic transaction without adding a waiter.""" diff --git a/tests/hand/api_v1/discovery/test_discovery.py b/tests/hand/api_v1/discovery/test_discovery.py index 967d8ad..146977c 100644 --- a/tests/hand/api_v1/discovery/test_discovery.py +++ b/tests/hand/api_v1/discovery/test_discovery.py @@ -286,7 +286,7 @@ def test_no_response_is_bounded_and_require_one_reports_no_hand() -> None: def test_known_incompatible_nominal_bitrate_is_not_probed() -> None: source = _Source( - _classic_channel(nominal_bitrate=500_000), + _classic_channel(nominal_bitrate=333_333), {}, ) diff --git a/tests/hand/api_v1/discovery/test_hop_six_discovery.py b/tests/hand/api_v1/discovery/test_hop_six_discovery.py new file mode 100644 index 0000000..05e26d3 --- /dev/null +++ b/tests/hand/api_v1/discovery/test_hop_six_discovery.py @@ -0,0 +1,164 @@ +"""Product-selected HOP discovery restores protocol, addresses, BRS and identity.""" + +from contextlib import closing +from dataclasses import replace + +import pytest + +from linkerbot_hand import ( + DeviceConfig, + DiscoveryConfig, + DiscoveryStatus, + HandSide, + IdentityMismatchError, + ProtocolKind, + TransportKind, + ValidationError, + discover_hands, +) +from tests.hand.api_v1.runtime.hop_six_fake import HOPSixScript +from tests.hand.api_v1.runtime.o30_canfd_fake import ScriptedO30Transport + +from .test_discovery import _FAST, _empty_transport, _fd_channel, _Source + + +@pytest.mark.parametrize("model", ["L6", "O6"]) +@pytest.mark.parametrize("side", [HandSide.LEFT, HandSide.RIGHT]) +@pytest.mark.parametrize( + "request_id,brs", [(1, False), (0x123, True), (0, False), (0x3FE, False)] +) +def test_discovery_preserves_hop_protocol_and_endpoint(model, side, request_id, brs): + script = HOPSixScript(model, side, request_id) + source = _Source( + _fd_channel(data_bitrate=2_000_000), + { + TransportKind.CAN: _empty_transport, + TransportKind.CAN_FD: lambda: ScriptedO30Transport(script), + }, + ) + report = discover_hands( + config=replace(_FAST, hop_request_ids=(request_id,), hop_brs=brs), + source=source, + ) + candidate = report.require_one() + assert candidate.model == model + assert candidate.side is side + assert candidate.capabilities.protocol is ProtocolKind.HOP + assert candidate.hop_brs is brs + assert not script.command_messages + from unittest.mock import patch + + with patch.object( + type(candidate.transport), + "create_transport", + return_value=ScriptedO30Transport(script), + ): + named = report.create_hand_devices() + hand = named[f"{model.lower()}{'l' if side is HandSide.LEFT else 'r'}"] + count = len(script.sent_messages) + assert hand.get_capabilities().protocol is ProtocolKind.HOP + assert len(script.sent_messages) == count + with closing(hand.open()): + assert hand.get_angles(timeout=1).raw == (10, 5, 11, 12, 13, 14) + assert all(m.arbitration_id == request_id for m in script.sent_messages) + assert all( + m.frame_type == (0x0C if brs else 0x04) for m in script.sent_messages + ) + script.device_uid = "DIFFERENT-DEVICE" + with pytest.raises(IdentityMismatchError): + hand.open() + assert not hand.get_snapshot().angles.meta.valid + + +@pytest.mark.parametrize("model", ["L6", "O6"]) +def test_missing_reported_side_is_partial_not_guessed_from_id(model): + script = HOPSixScript(model, side=None) + source = _Source( + _fd_channel(), + { + TransportKind.CAN: _empty_transport, + TransportKind.CAN_FD: lambda: ScriptedO30Transport(script), + }, + ) + report = discover_hands(config=replace(_FAST, hop_request_ids=(1,)), source=source) + assert report.create_hand_devices() == {} + candidate = report.require_one(allow_partial=True) + assert candidate.status is DiscoveryStatus.PARTIAL + assert candidate.side is None + assert candidate.problems[0].code == "missing_side" + # Explicit side remains the documented manual override for missing-side candidates. + from unittest.mock import patch + + with patch.object( + type(candidate.transport), + "create_transport", + return_value=ScriptedO30Transport(script), + ): + hand = candidate.create_hand_device(side="left") + with closing(hand.open()): + assert hand.get_device_info(timeout=1).reported_side is None + + +@pytest.mark.parametrize("ids", [(), (1, 1), (0x3FF,), (True,), [1]]) +def test_invalid_hop_discovery_options(ids): + with pytest.raises(ValidationError): + DiscoveryConfig(hop_request_ids=ids) + + +def test_hop_options_reject_conflicting_discovered_id(): + script = HOPSixScript() + source = _Source( + _fd_channel(), + { + TransportKind.CAN: _empty_transport, + TransportKind.CAN_FD: lambda: ScriptedO30Transport(script), + }, + ) + candidate = discover_hands( + config=replace(_FAST, hop_request_ids=(1,)), source=source + ).require_one() + with pytest.raises(ValidationError, match="disagrees"): + candidate.create_hand_device(config=DeviceConfig(hop_request_id=0x123)) + + +def test_configured_nominal_bitrate_is_discoverable_without_retuning(): + from unittest.mock import patch + + script = HOPSixScript() + script.configuration[8] = 3 + source = _Source( + replace(_fd_channel(), nominal_bitrate=500_000), + { + TransportKind.CAN: _empty_transport, + TransportKind.CAN_FD: lambda: ScriptedO30Transport(script), + }, + ) + report = discover_hands(replace(_FAST, hop_request_ids=(1,)), source=source) + candidate = report.require_one() + assert candidate.transport.nominal_bitrate == 500_000 + with patch.object( + type(candidate.transport), + "create_transport", + return_value=ScriptedO30Transport(script), + ): + hand = candidate.create_hand_device() + with closing(hand.open()): + assert hand.get_can_configuration().nominal_bitrate == 500_000 + + +def test_unknown_hop_product_is_partial_with_unknown_model(): + script = HOPSixScript() + script.product_model = "UNKNOWN" + source = _Source( + _fd_channel(), + { + TransportKind.CAN: _empty_transport, + TransportKind.CAN_FD: lambda: ScriptedO30Transport(script), + }, + ) + report = discover_hands(replace(_FAST, hop_request_ids=(1,)), source=source) + candidate = report.require_one(allow_partial=True) + assert candidate.model is None + assert candidate.status is DiscoveryStatus.PARTIAL + assert candidate.problems[0].code == "unsupported_product" + assert report.create_hand_devices() == {} diff --git a/tests/hand/api_v1/discovery/test_o30_classic.py b/tests/hand/api_v1/discovery/test_o30_classic.py new file mode 100644 index 0000000..6fee4ca --- /dev/null +++ b/tests/hand/api_v1/discovery/test_o30_classic.py @@ -0,0 +1,102 @@ +"""O30 classic CAN discovery preserves wire mode, addressing and diagnostics.""" + +from dataclasses import replace + +import pytest + +from linkerbot_hand import ( + DeviceConfig, + DiscoveryStatus, + HandSide, + TransportKind, + ValidationError, + discover_hands, +) +from tests.hand.api_v1.runtime.o30_canfd_fake import O30DeviceScript +from tests.hand.api_v1.runtime.o30_classic_fake import ClassicO30Transport + +from .test_discovery import _FAST, _classic_channel, _Source + + +@pytest.mark.parametrize("bitrate", [1_000_000, 500_000]) +@pytest.mark.parametrize("side", [HandSide.LEFT, None]) +def test_discovery_preserves_classic_mode_custom_endpoint_and_actual_side( + bitrate, side +): + script = O30DeviceScript( + product_model="O30", reported_side=side, request_id=0x123, response_id=0x523 + ) + source = _Source( + _classic_channel(nominal_bitrate=bitrate), + {TransportKind.CAN: lambda: ClassicO30Transport(script)}, + ) + report = discover_hands(replace(_FAST, hop_request_ids=(0x123,)), source=source) + assert not report.failures + c = report.require_one(allow_partial=True) + assert ( + c.wire_protocol is TransportKind.CAN and c.transport.kind is TransportKind.CAN + ) + assert c.side is side + assert c.status is ( + DiscoveryStatus.PARTIAL if side is None else DiscoveryStatus.VERIFIED + ) + if bitrate != 1_000_000: + assert all(len(m.data) >= 3 for _, t in source.created for m in t.requests) + with pytest.raises(ValidationError, match="hop_transport"): + c.create_hand_device( + config=DeviceConfig(), side="right" if side is None else None + ) + hand = c.create_hand_device(side="right" if side is None else None) + try: + hand.open() + assert hand.get_capabilities().transport.kind is TransportKind.CAN + assert hand.get_angles(timeout=1).raw == tuple(range(20)) + finally: + hand.close() + + +def test_classic_discovery_preserves_device_error_instead_of_timing_out(): + class RejectInfo(O30DeviceScript): + def _read(self, main_index, sub_index, length, *, rts): + if main_index == 0x41 and sub_index == 0x10: + return (self._error(0, 0x10),) + return super()._read(main_index, sub_index, length, rts=rts) + + source = _Source( + _classic_channel(), + {TransportKind.CAN: lambda: ClassicO30Transport(RejectInfo())}, + ) + report = discover_hands(replace(_FAST, hop_request_ids=(1,)), source=source) + candidate = report.require_one(allow_partial=True) + assert candidate.status is DiscoveryStatus.PARTIAL + assert candidate.problems[0].code != "identity_timeout" + assert "permission denied" in candidate.problems[0].message + + +@pytest.mark.parametrize("first_si", [5, 10, 15]) +def test_missing_first_product_fragment_keeps_partial_endpoint(first_si): + class NoFirstProduct(O30DeviceScript): + def _read(self, main_index, sub_index, length, *, rts): + if main_index == 0x41: + if sub_index != 0: + return () + return ( + self._response( + main_index, + first_si, + self._identity_object(main_index)[first_si:16], + ), + ) + return super()._read(main_index, sub_index, length, rts=rts) + + source = _Source( + _classic_channel(), + {TransportKind.CAN: lambda: ClassicO30Transport(NoFirstProduct())}, + ) + report = discover_hands(replace(_FAST, hop_request_ids=(1,)), source=source) + candidate = report.require_one(allow_partial=True) + assert candidate.status is DiscoveryStatus.PARTIAL + assert candidate.model is None + assert candidate.address.request_id == 1 + assert candidate.problems[0].code == "product_code_incomplete" + assert not report.failures diff --git a/tests/hand/api_v1/runtime/hop_six_fake.py b/tests/hand/api_v1/runtime/hop_six_fake.py new file mode 100644 index 0000000..4ae8067 --- /dev/null +++ b/tests/hand/api_v1/runtime/hop_six_fake.py @@ -0,0 +1,76 @@ +"""Six-joint HOP script with independent actual/configured register contents.""" + +from linkerbot_hand import HandSide +from tests.hand.api_v1.runtime.o30_canfd_fake import O30DeviceScript + + +class HOPSixScript(O30DeviceScript): + def __init__( + self, model="L6", side=HandSide.RIGHT, request_id=1, sensor_variant="NO_SENSOR" + ): + super().__init__( + product_model=model, + device_uid=f"UID-{model}", + identity_layout="documented", + reported_side=side, + request_id=request_id, + response_id=request_id | 0x400, + sensor_variant=sensor_variant, + ) + for mi in range(1, 13): + self.actual[mi] = bytes(range(36)) if mi != 12 else bytes(36) + self.targets[mi] = bytes(range(100, 136)) + for mi in (0x21, 0x23): + self.actual[mi] = self.targets[mi] = b"".join( + n.to_bytes(2, "little") for n in range(36) + ) + self.configuration = bytearray(32) + self.configuration[:2] = request_id.to_bytes(2, "little") + self.configuration[6:10] = bytes([2, 0, 1, 5]) + self.drop_range = None + self.reject_range = None + self.drop_configuration_ack = True + + def _identity_object(self, main_index): + if main_index == 0x35: + return bytes(self.configuration) + return super()._identity_object(main_index) + + def _read(self, main_index, sub_index, length, *, rts): + if (main_index, sub_index) == self.drop_range: + return () + if (main_index, sub_index) == self.reject_range: + return (self._error(sub_index, 2),) + return super()._read(main_index, sub_index, length, rts=rts) + + def _write(self, main_index, sub_index, length, payload, *, rts=False): + if main_index == 0x31: + return super()._write(main_index, sub_index, length, payload, rts=rts) + if main_index == 0x35: + self.configuration[sub_index : sub_index + length] = payload[:length] + if sub_index == 0: + self.request_id = int.from_bytes(payload[:2], "little") + self.response_id = self.request_id | 0x400 + if sub_index != 0x1E and self.drop_configuration_ack: + return () + return (self._response(main_index, sub_index, payload[:length], rts=rts),) + if main_index not in self.targets: + return (self._error(sub_index, 2),) + width = 2 if main_index in (0x21, 0x23) else 1 + if (sub_index, length) not in ((5 * width, width), (10 * width, 5 * width)): + return (self._error(sub_index, 0x40),) + if (main_index, sub_index) == self.drop_range: + return () + if (main_index, sub_index) == self.reject_range: + return (self._error(sub_index, 2),) + target = bytearray(self.targets[main_index]) + target[sub_index : sub_index + length] = payload[:length] + self.targets[main_index] = bytes(target) + if main_index == 1: + self.actual[main_index] = bytes(target) + data = (self.targets if rts else self.actual)[main_index] + return ( + self._response( + main_index, sub_index, data[sub_index : sub_index + length], rts=rts + ), + ) diff --git a/tests/hand/api_v1/runtime/o30_classic_fake.py b/tests/hand/api_v1/runtime/o30_classic_fake.py new file mode 100644 index 0000000..567bfce --- /dev/null +++ b/tests/hand/api_v1/runtime/o30_classic_fake.py @@ -0,0 +1,86 @@ +"""Classic CAN adapter for the shared O30 object-model test fixture.""" + +from __future__ import annotations + +import can + +from linkerbot_transport import CANRouteKey, SendReceipt +from linkerbot_transport.canfd import CANFDMessage +from linkerbot_transport.testing import FakeTransport + +from .o30_canfd_fake import _WRITABLE_OBJECTS, O30DeviceScript + + +class ClassicO30Transport(FakeTransport[can.Message]): + """Split replies into five-byte payloads and apply range writes in place.""" + + def __init__( + self, script: O30DeviceScript, *, drop_write_si=None, error_write_si=None + ): + super().__init__( + route_key_of=lambda m: CANRouteKey(m.arbitration_id, m.is_extended_id) + ) + self.script = script + self.requests: list[can.Message] = [] + self.drop_write_si = drop_write_si + self.error_write_si = error_write_si + + def send(self, message: can.Message) -> SendReceipt: + assert isinstance(message, can.Message) + assert not message.is_fd and not message.bitrate_switch + assert len(message.data) <= 8 + receipt = super().send(message) + if receipt.exception() is not None: + return receipt + self.requests.append(message) + data = bytes(message.data) + if message.arbitration_id != self.script.request_id or len(data) < 3: + return receipt + mi, si, count = data[:3] + if len(data) > 3 and mi & 0x7F in _WRITABLE_OBJECTS: + if si == self.drop_write_si: + return receipt + if si == self.error_write_si: + replies = (self.script._error(si, 0x10),) + else: + target = bytearray(self.script.targets[mi & 0x7F]) + target[si : si + count] = data[3:] + replies = self.script.respond( + CANFDMessage( + arbitration_id=message.arbitration_id, + data=bytes((mi, 0, 25)) + bytes(target), + is_extended_id=False, + frame_type=4, + ) + ) + replies = tuple( + CANFDMessage( + arbitration_id=r.arbitration_id, + data=bytes((mi, si, count)) + r.data[3 + si : 3 + si + count], + is_extended_id=False, + frame_type=4, + ) + for r in replies + ) + else: + replies = self.script.respond( + CANFDMessage( + arbitration_id=message.arbitration_id, + data=data, + is_extended_id=False, + frame_type=4, + ) + ) + for reply in replies: + control, start, size = reply.data[:3] + payload = reply.data[3 : 3 + size] + for offset in range(0, len(payload), 5): + chunk = payload[offset : offset + 5] + self.emit( + can.Message( + arbitration_id=reply.arbitration_id, + data=bytes((control, start + offset, len(chunk))) + chunk, + is_extended_id=False, + ) + ) + return receipt diff --git a/tests/hand/api_v1/runtime/test_hop_six.py b/tests/hand/api_v1/runtime/test_hop_six.py new file mode 100644 index 0000000..468c677 --- /dev/null +++ b/tests/hand/api_v1/runtime/test_hop_six.py @@ -0,0 +1,653 @@ +"""Public L6/O6 dual-protocol, HOP object and lifecycle regressions.""" + +from contextlib import closing + +import pytest + +from linkerbot_hand import ( + DeviceConfig, + HandDevice, + HandSide, + IdentityMismatchError, + JointFaultStatus, + LifecycleError, + PollingPlan, + ProtocolError, + ProtocolKind, + TimeoutError, + TransportKind, + UnsupportedOperationError, + ValidationError, + get_declared_capabilities, +) +from linkerbot_hand.api.commands import CommandStage +from linkerbot_hand.protocols.hop.framing import HOPAddressing +from linkerbot_hand.protocols.hop.management import configuration_transaction +from linkerbot_hand.protocols.hop.six_axis import POSITIONS +from linkerbot_hand.protocols.registry import create_protocol_backend +from tests.hand.api_v1.runtime.hop_six_fake import HOPSixScript +from tests.hand.api_v1.runtime.o30_canfd_fake import ScriptedO30Transport + + +def make_hand( + model="L6", side=HandSide.RIGHT, request_id=1, sensor_variant="NO_SENSOR", brs=False +): + script = HOPSixScript(model, side, request_id, sensor_variant) + transport = ScriptedO30Transport(script) + hand = HandDevice( + model=model, + side=side, + protocol="hop", + transport=transport, + config=DeviceConfig(hop_request_id=request_id, hop_brs=brs), + ) + return script, transport, hand + + +@pytest.mark.parametrize("model", ["L6", "O6"]) +@pytest.mark.parametrize("side", [HandSide.LEFT, HandSide.RIGHT]) +def test_dual_protocol_profiles_and_default_ids(model, side): + legacy = get_declared_capabilities(model, side=side) + hop = get_declared_capabilities(model, side=side, protocol="hop") + assert legacy.protocol is ProtocolKind.LEGACY + assert legacy.transport.kind is TransportKind.CAN + assert hop.protocol is ProtocolKind.HOP + assert hop.transport.kind is TransportKind.CAN_FD + assert hop.joint_names == legacy.joint_names + assert create_protocol_backend(hop).route_keys[0].arbitration_id == 0x401 + script, _, hand = make_hand(model, side) + assert not script.sent_messages + with closing(hand.open()): + assert hand.get_device_info(timeout=1).reported_side is side + assert hand.get_angles(timeout=1).raw == POSITIONS + assert all( + m.arbitration_id == 1 and m.frame_type == 0x04 for m in script.sent_messages + ) + + +@pytest.mark.parametrize("model", ["L6", "O6"]) +@pytest.mark.parametrize( + "name,mi,rts", + [ + ("angles", 1, False), + ("speeds", 2, False), + ("configured_speeds", 2, True), + ("configured_accelerations", 3, True), + ("currents", 4, False), + ("current_limits", 4, True), + ("voltages", 5, False), + ("torques", 6, False), + ("torques_limits", 6, True), + ("temperatures", 7, False), + ("motion_times", 8, True), + ("stall_detection_times", 9, True), + ("stall_thresholds", 10, True), + ("stall_hold_currents", 11, True), + ], +) +def test_reads_actual_and_configured_views(model, name, mi, rts): + script, _, hand = make_hand(model) + with closing(hand.open()): + sample = getattr(hand, f"get_{name}")(timeout=1) + expected = tuple((100 if rts else 0) + pos for pos in POSITIONS) + assert sample.raw == expected + assert script.sent_messages[-1].data[:3] == bytes( + [mi | (0x80 if rts else 0), 10, 5] + ) + + +@pytest.mark.parametrize("model", ["L6", "O6"]) +@pytest.mark.parametrize( + "name", + [ + "angles", + "speeds", + "accelerations", + "current_limits", + "torques_limits", + "motion_times", + "stall_detection_times", + "stall_thresholds", + "stall_hold_currents", + ], +) +def test_write_preserves_holes_and_completes_all_ranges(model, name): + script, _, hand = make_hand(model) + with closing(hand.open()): + before = dict(script.targets) + handle = getattr(hand, f"set_{name}")([0, 1, 2, 3, 254, 255]) + handle.wait(1) + assert handle.stage is CommandStage.ACKNOWLEDGED + messages = script.command_messages[-2:] + assert [m.data[1:3] for m in messages] == [b"\x05\x01", b"\x0a\x05"] + mi = messages[0].data[0] & 0x7F + assert script.targets[mi][5] == 1 + assert script.targets[mi][10:15] == bytes([0, 2, 3, 254, 255]) + assert script.targets[mi][:5] == before[mi][:5] + assert script.targets[mi][6:10] == before[mi][6:10] + if name == "angles": + assert hand.get_snapshot().angles.raw == (0, 1, 2, 3, 254, 255) + + +def test_l6_pid_is_u16_and_torque_zero_has_no_legacy_mode_write(): + script, _, hand = make_hand() + with closing(hand.open()): + for name, mi in (("position_kp", 0x21), ("position_kd", 0x23)): + getattr(hand, f"set_{name}")([0, 1, 2, 3, 4, 255]).wait(1) + assert script.command_messages[-2].data[1:3] == bytes([10, 2]) + assert script.command_messages[-1].data[1:3] == bytes([20, 10]) + assert getattr(hand, f"get_{name}")(timeout=1).raw == (0, 1, 2, 3, 4, 255) + with pytest.raises(ValidationError): + getattr(hand, f"set_{name}")([256] * 6) + hand.set_torques([0] * 6).wait(1) + assert all(m.data[0] & 0x7F == 6 for m in script.command_messages[-2:]) + + +@pytest.mark.parametrize( + "model,name", + [ + ("L6", "clear_faults"), + ("L6", "set_torque_kd"), + ("L6", "set_limit_compensations"), + ("O6", "set_position_kp"), + ("O6", "set_torques"), + ("L6", "enable"), + ("O6", "calibrate"), + ], +) +def test_undefined_operations_fail_without_wire_io(model, name): + script, _, hand = make_hand(model) + with closing(hand.open()): + count = len(script.sent_messages) + with pytest.raises(UnsupportedOperationError): + getattr(hand, name)(*[([0] * 6)] if name.startswith("set_") else []) + assert len(script.sent_messages) == count + + +def test_hop_fault_bits_and_incomplete_sample_does_not_replace_cache(): + script, _, hand = make_hand() + script.actual[12] = bytes([0xCF] * 36) + with closing(hand.open()): + sample = hand.get_faults(timeout=1) + assert all(isinstance(s, JointFaultStatus) for s in sample.statuses) + assert sample.statuses[0].over_current + before = hand.get_snapshot().angles + script.drop_range = (1, 10) + with pytest.raises(TimeoutError): + hand.get_angles(timeout=0.3) + assert hand.get_snapshot().angles.raw == before.raw + assert hand.get_snapshot().angles.meta.sequence == before.meta.sequence + + +def test_second_write_range_failure_is_not_acknowledged(): + script, _, hand = make_hand() + with closing(hand.open()): + script.reject_range = (1, 10) + handle = hand.set_angles([42] * 6) + assert handle.wait(1) is CommandStage.FAILED + assert handle.error is not None + assert script.targets[1][5] == 42 + assert script.targets[1][10] != 42 + assert hand.get_snapshot().angles.raw == POSITIONS + + +@pytest.mark.parametrize( + "variant,fields", + [("TASHAN_GATHER", ("Fn", "Ft", "Fd", "Px")), ("TSSP_JZG", ("FnS", "Fn"))], +) +@pytest.mark.parametrize("model", ["L6", "O6"]) +def test_shared_sensor_descriptor_values(model, variant, fields): + _, _, hand = make_hand(model, sensor_variant=variant) + with closing(hand.open()): + sample = hand.get_force(*(f.lower() for f in fields), timeout=2) + assert len(sample.sensors) == 5 + assert tuple(f.semantic for f in sample.sensors[0].fields) == fields + + +@pytest.mark.parametrize( + "name,args,kwargs", + [ + ("set_can_id", (0x123,), {}), + ("set_can_brs", (True,), {}), + ("set_can_bitrates", (), {"data_bitrate": 2_000_000}), + ], +) +def test_configuration_rejected_while_polling_and_can_continue(name, args, kwargs): + script, _, hand = make_hand() + with closing(hand.open()): + hand.start_polling(PollingPlan(angles_hz=30)) + assert hand.get_can_configuration().request_id == 1 + with pytest.raises(LifecycleError, match="stop_polling"): + getattr(hand, name)(*args, **kwargs) + assert not any(m.data[0] & 0x7F == 0x35 for m in script.command_messages) + assert hand.get_angles(timeout=1).raw == POSITIONS + hand.stop_polling() + getattr(hand, name)(*args, **kwargs) + assert not hand.get_snapshot().angles.meta.valid + + +@pytest.mark.parametrize( + "name,args,kwargs,si,payload", + [ + ("set_can_id", (0x123,), {}, 0, b"\x23\x01"), + ("set_can_brs", (True,), {}, 7, b"\x01"), + ("set_can_brs", (False,), {}, 7, b"\x00"), + ( + "set_can_bitrates", + (), + {"nominal_bitrate": 500_000, "data_bitrate": 2_000_000}, + 8, + b"\x03\x05", + ), + ("set_can_bitrates", (), {"data_bitrate": 5_000_000}, 9, b"\x02"), + ], +) +@pytest.mark.parametrize("model", ["L6", "O6"]) +def test_configuration_single_dispatch_no_ack_and_closed( + name, args, kwargs, si, payload, model +): + script, _, hand = make_hand(model) + hand.open() + try: + config = hand.get_can_configuration() + assert (config.request_id, config.response_id, config.brs) == (1, 0x401, False) + assert config.nominal_bitrate == 1_000_000 + count = len(script.command_messages) + getattr(hand, name)(*args, **kwargs) + assert len(script.command_messages) == count + 1 + assert ( + script.command_messages[-1].data + == bytes([0xB5, si, len(payload)]) + payload + ) + assert not hand.get_snapshot().angles.meta.valid + finally: + hand.close() + + +@pytest.mark.parametrize("model", ["L6", "O6"]) +def test_new_config_reconnect_and_save(model): + script, transport, hand = make_hand(model) + hand.open() + hand.set_can_id(0x123) + new = HandDevice( + model=model, + protocol="hop", + side="right", + transport=transport, + config=DeviceConfig(hop_request_id=0x123, hop_brs=True), + ) + with closing(new.open()): + assert new.get_can_configuration().request_id == 0x123 + new.save_parameters().wait(1) + assert script.command_messages[-1].data == b"\xb5\x1e\x01\x01" + assert all( + m.frame_type == 0x0C + for m in script.sent_messages + if m.arbitration_id == 0x123 + ) + + +@pytest.mark.parametrize("value", [-1, 0x3FF, True, 1.0]) +def test_invalid_id_has_no_side_effects(value): + script, _, hand = make_hand() + with closing(hand.open()): + count = len(script.sent_messages) + with pytest.raises(ValidationError): + hand.set_can_id(value) + assert len(script.sent_messages) == count + assert hand.get_angles(timeout=1).meta.valid + + +@pytest.mark.parametrize( + "nominal_code,data_code,nominal,data", + [ + (1, 1, 1_000_000, 8_000_000), + (3, 5, 500_000, 2_000_000), + (8, 13, 100_000, 100_000), + (2, 6, 800_000, 1_000_000), + ], +) +def test_configuration_bitrate_enums_roundtrip(nominal_code, data_code, nominal, data): + script = HOPSixScript() + script.configuration[8:10] = bytes([nominal_code, data_code]) + tx = configuration_transaction(HOPAddressing(1, 0x401)) + while tx is not None: + progress = tx.consume(script.respond(tx.request_messages[0])[0]) + result = progress.result + tx = tx.build_next_transaction() + assert result.nominal_bitrate == nominal + assert result.data_bitrate == data + + +def test_side_mismatch_rejected_before_baseline(): + script, transport, _ = make_hand(side=HandSide.LEFT) + hand = HandDevice(model="L6", side="right", protocol="hop", transport=transport) + with pytest.raises(IdentityMismatchError): + hand.open() + assert not any(m.data[0] & 0x7F == 1 for m in script.sent_messages) + + +def test_reconfiguration_serializes_with_poll_start_and_rejects_concurrent_work(): + import threading + + from linkerbot_hand import ClosedError, DeviceNotReadyError + + script, _, hand = make_hand() + entered = threading.Event() + release = threading.Event() + attempting_poll = threading.Event() + errors = [] + original_write = script._write + + def block_configuration(mi, si, length, payload, *, rts=False): + if mi == 0x35: + entered.set() + if not release.wait(2): + raise RuntimeError("test release was not signaled") + return original_write(mi, si, length, payload, rts=rts) + + script._write = block_configuration + hand.open() + + def change(): + try: + hand.set_can_brs(True) + except Exception as error: # noqa: BLE001 - propagate worker failures to test + errors.append(error) + + def poll(): + attempting_poll.set() + try: + hand.start_polling(PollingPlan(angles_hz=30)) + except ClosedError: + return + errors.append(RuntimeError("polling started during reconfiguration")) + + writer = threading.Thread(target=change) + starter = threading.Thread(target=poll) + try: + writer.start() + assert entered.wait(1) + with pytest.raises(DeviceNotReadyError): + hand.set_angles([0] * 6) + with pytest.raises(DeviceNotReadyError): + hand.get_angles(timeout=0.2) + starter.start() + assert attempting_poll.wait(1) + release.set() + writer.join(2) + starter.join(2) + assert not writer.is_alive() and not starter.is_alive() + assert not errors + assert script.sent_messages[-1].data == b"\xb5\x07\x01\x01" + finally: + release.set() + writer.join(2) + if starter.ident is not None: + starter.join(2) + hand.close() + + +@pytest.mark.parametrize("model", ["L6", "O6"]) +@pytest.mark.parametrize( + "name,args,kwargs", + [ + ("set_can_id", (0x123,), {}), + ("set_can_brs", (True,), {}), + ("set_can_bitrates", (), {"data_bitrate": 2_000_000}), + ], +) +def test_configuration_switch_rejects_reads_before_old_waiters_are_notified( + monkeypatch, model, name, args, kwargs +): + import threading + + from linkerbot_hand import DeviceNotReadyError, ErrorCode + from linkerbot_hand.capabilities.types import TelemetrySemantic + + script, _, hand = make_hand(model) + hand.open() + runtime = hand._runtime + coordinator = runtime._transactions + original_read = script._read + original_request = coordinator.request + inflight_started = threading.Event() + finish_inflight = threading.Event() + queued = threading.Event() + notifying = threading.Event() + finish_notifications = threading.Event() + errors = {} + count = len(script.sent_messages) + + def block_inflight_read(mi, si, length, *, rts): + if mi == 1 and si == 5: + inflight_started.set() + assert finish_inflight.wait(3), "in-flight read was not released" + return original_read(mi, si, length, rts=rts) + + def observe_request(key, **request_kwargs): + waiter = original_request(key, **request_kwargs) + if key is TelemetrySemantic.CURRENTS: + original_fail = waiter.fail + + def pause_notification(error): + original_fail(error) + notifying.set() + assert finish_notifications.wait(3), "notification was not released" + + monkeypatch.setattr(waiter, "fail", pause_notification) + queued.set() + return waiter + + def call(label, function): + try: + function() + except Exception as error: # noqa: BLE001 - collect worker failures + errors[label] = error + + monkeypatch.setattr(script, "_read", block_inflight_read) + monkeypatch.setattr(coordinator, "request", observe_request) + threads = [ + threading.Thread( + target=call, args=("inflight", lambda: hand.get_angles(timeout=3)) + ), + threading.Thread( + target=call, args=("queued", lambda: hand.get_currents(timeout=3)) + ), + threading.Thread( + target=call, + args=("writer", lambda: getattr(hand, name)(*args, timeout=2, **kwargs)), + ), + ] + try: + threads[0].start() + assert inflight_started.wait(1) + threads[1].start() + assert queued.wait(1) + command = hand.set_speeds([25] * 6) + threads[2].start() + assert notifying.wait(1) + # The actor is still finishing the old read, while its caller and the + # queued caller are being failed. New readers must already be rejected. + for read in ( + hand.get_angles, + hand.get_currents, + hand.get_can_configuration, + hand.get_device_info, + ): + with pytest.raises(DeviceNotReadyError, match="CAN settings are changing"): + read(timeout=0.1) + with pytest.raises(DeviceNotReadyError): + hand.set_angles([0] * 6) + assert command.stage is CommandStage.FAILED + assert command.error is not None + assert command.error.code is ErrorCode.DEVICE_NOT_READY + finish_notifications.set() + finish_inflight.set() + for thread in threads: + thread.join(3) + assert all(not thread.is_alive() for thread in threads) + assert "writer" not in errors + assert set(errors) == {"inflight", "queued"} + assert all(isinstance(error, DeviceNotReadyError) for error in errors.values()) + messages = script.sent_messages[count:] + assert [(m.data[0] & 0x7F, m.data[1]) for m in messages[:2]] == [ + (1, 5), + (1, 10), + ] + assert len(messages) == 3 + assert len(script.command_messages) == 1 + assert messages[-1].data[0] == 0xB5 + assert not hand.get_snapshot().angles.meta.valid + finally: + finish_notifications.set() + finish_inflight.set() + for thread in threads: + if thread.ident is not None: + thread.join(3) + hand.close() + + +def test_configuration_preparation_failure_closes_the_session(monkeypatch): + from linkerbot_hand import ClosedError + + script, _, hand = make_hand() + hand.open() + commands = hand._runtime._commands + original_fail_pending = commands.fail_pending + failure = RuntimeError("configuration preparation failed") + calls = 0 + + def fail_once(error): + nonlocal calls + calls += 1 + if calls == 1: + raise failure + return original_fail_pending(error) + + monkeypatch.setattr(commands, "fail_pending", fail_once) + try: + with pytest.raises(RuntimeError) as raised: + hand.set_can_brs(True) + assert raised.value is failure + assert not script.command_messages + assert not hand.get_snapshot().angles.meta.valid + with pytest.raises(ClosedError): + hand.get_angles(timeout=0.1) + finally: + hand.close() + + +@pytest.mark.parametrize("model", ["L6", "O6"]) +@pytest.mark.parametrize( + "name,args,kwargs", + [ + ("set_can_id", (0x123,), {}), + ("set_can_brs", (True,), {}), + ("set_can_bitrates", (), {"data_bitrate": 2_000_000}), + ], +) +def test_configuration_timeout_before_dispatch_does_not_send_later( + model, name, args, kwargs +): + import threading + + script, _, hand = make_hand(model) + hand.open() + entered = threading.Event() + read_errors = [] + original_read = script._read + + def dropped_read(mi, si, length, *, rts): + if mi == 1: + entered.set() + return () + return original_read(mi, si, length, rts=rts) + + script._read = dropped_read + + def read(): + try: + hand.get_angles(timeout=1) + except Exception as error: # noqa: BLE001 - propagate worker failures to test + read_errors.append(error) + + reader = threading.Thread(target=read) + try: + reader.start() + assert entered.wait(1) + with pytest.raises(TimeoutError): + getattr(hand, name)(*args, timeout=0.01, **kwargs) + reader.join(1) + assert not reader.is_alive() + assert read_errors + assert not any(m.data[0] & 0x7F == 0x35 for m in script.command_messages) + finally: + hand.close() + reader.join(1) + + +@pytest.mark.parametrize( + "name,args,kwargs", + [ + ("set_can_brs", (1,), {}), + ("set_can_brs", (None,), {}), + ("set_can_bitrates", (), {}), + ("set_can_bitrates", (), {"nominal_bitrate": 123456}), + ("set_can_bitrates", (), {"data_bitrate": True}), + ("set_can_bitrates", (), {"nominal_bitrate": 1_000_000.0}), + ("set_can_id", (1,), {"timeout": 0}), + ], +) +def test_invalid_configuration_arguments_keep_connection_usable(name, args, kwargs): + script, _, hand = make_hand() + with closing(hand.open()): + count = len(script.sent_messages) + with pytest.raises(ValidationError): + getattr(hand, name)(*args, **kwargs) + assert len(script.sent_messages) == count + assert hand.get_angles(timeout=1).raw == POSITIONS + + +@pytest.mark.parametrize("index,value", [(6, 0), (7, 2), (8, 0), (9, 14)]) +def test_invalid_device_configuration_is_reported_as_protocol_error(index, value): + script, _, hand = make_hand() + script.configuration[index] = value + with closing(hand.open()): + with pytest.raises(ProtocolError): + hand.get_can_configuration() + assert hand.get_angles(timeout=1).raw == POSITIONS + + +def test_installed_empty_polling_plan_also_blocks_configuration(): + script, _, hand = make_hand() + with closing(hand.open()): + hand.start_polling(PollingPlan()) + with pytest.raises(LifecycleError, match="stop_polling"): + hand.set_can_brs(True) + assert not script.command_messages + hand.stop_polling() + hand.set_can_brs(True) + + +@pytest.mark.parametrize( + "model,protocol", [("L6", None), ("O6", None), ("O30", None), ("L20", None)] +) +def test_device_register_api_is_not_assumed_for_other_profiles(model, protocol): + hand = HandDevice(model=model, side="right", protocol=protocol, transport=object()) + for fn in [ + lambda: hand.get_can_configuration(), + lambda: hand.set_can_id(1), + lambda: hand.set_can_brs(True), + lambda: hand.set_can_bitrates(data_bitrate=2_000_000), + ]: + with pytest.raises(UnsupportedOperationError): + fn() + + +@pytest.mark.parametrize( + "model,protocol", [("L6", "auto"), ("O30", "legacy"), ("L20", "hop")] +) +def test_protocol_selection_rejects_unsupported_profiles(model, protocol): + with pytest.raises(ValidationError): + HandDevice(model=model, side="right", protocol=protocol, transport=object()) diff --git a/tests/hand/api_v1/runtime/test_o30_classic_can.py b/tests/hand/api_v1/runtime/test_o30_classic_can.py new file mode 100644 index 0000000..efbc9b5 --- /dev/null +++ b/tests/hand/api_v1/runtime/test_o30_classic_can.py @@ -0,0 +1,337 @@ +"""O30 classic HOP framing, command failures, sensors and range-read regressions.""" + +import can +import pytest + +from linkerbot_hand import ( + CommandStage, + DeviceConfig, + HandDevice, + HandSide, + TransportKind, + UnsupportedFirmwareError, + ValidationError, + get_declared_capabilities, +) +from linkerbot_hand.api.errors import ProtocolError +from linkerbot_hand.capabilities.types import CommandSemantic, TelemetrySemantic +from linkerbot_hand.protocols.hop.framing import ( + build_read_message, + build_write_message, + matching_response_frame, +) +from linkerbot_hand.protocols.hop.response import HOPRangeAssembler +from linkerbot_hand.protocols.o30 import O30ProtocolBackend +from linkerbot_hand.protocols.o30.codec import encode_v25 + +from .o30_canfd_fake import O30DeviceScript +from .o30_classic_fake import ClassicO30Transport + + +def make_hand(script=None, **transport_options): + transport = ClassicO30Transport(script or O30DeviceScript(), **transport_options) + hand = HandDevice( + model="O30", + side="right", + transport=transport, + config=DeviceConfig(hop_transport=TransportKind.CAN), + ) + return hand, transport + + +@pytest.mark.parametrize("value", ["can", None, True, 1]) +def test_mode_requires_transport_enum(value): + with pytest.raises(ValidationError): + DeviceConfig(hop_transport=value) + + +def test_default_fd_and_classic_validation(): + assert DeviceConfig().hop_transport is TransportKind.CAN_FD + with pytest.raises(ValidationError, match="BRS"): + DeviceConfig(hop_transport=TransportKind.CAN, hop_brs=True) + for model in ("L6", "O6", "L20", "O20", "L30"): + with pytest.raises(ValidationError, match="requires O6 or O30"): + HandDevice( + model=model, + side=None if model == "L30" else "right", + transport=object(), + config=DeviceConfig(hop_transport=TransportKind.CAN), + ) + for model in ("L6",): + with pytest.raises(ValidationError): + get_declared_capabilities( + model, side="right", protocol="hop", transport_kind="can" + ) + + +def test_classic_frame_limits_and_response_filtering(): + read = build_read_message(1, sub_index=0, length=25, request_id=1, can_fd=False) + assert isinstance(read, can.Message) and bytes(read.data) == b"\x01\x00\x19" + write = build_write_message( + 1, sub_index=5, payload=bytes(5), request_id=1, can_fd=False + ) + assert len(write.data) == 8 and not write.is_fd + with pytest.raises(ValidationError, match="5 bytes"): + build_write_message( + 1, sub_index=0, payload=bytes(6), request_id=1, can_fd=False + ) + for flags in ( + {"is_error_frame": True}, + {"is_remote_frame": True}, + {"is_fd": True}, + {"is_extended_id": True}, + ): + msg = can.Message(arbitration_id=0x401, data=b"\x01\x00\x01\x01", **flags) + assert matching_response_frame(msg, 0x401) is None + + +def test_unordered_duplicate_and_conflicting_classic_fragments(): + assembler = HOPRangeAssembler( + 1, sub_index=0, length=25, rts=False, response_id=0x401 + ) + wire = encode_v25(tuple(range(20))) + + def frame(si, payload): + return can.Message( + arbitration_id=0x401, + data=bytes((1, si, len(payload))) + payload, + is_extended_id=False, + ) + + for si in (20, 5, 5, 15, 0): + assert assembler.consume(frame(si, wire[si : si + 5]))[1] is None + _, result = assembler.consume(frame(10, wire[10:15])) + assert result == wire + with pytest.raises(ProtocolError, match="conflicts"): + assembler.consume(frame(0, b"\xff" + wire[1:5])) + + +@pytest.mark.parametrize( + "semantic", + [CommandSemantic.ANGLES, CommandSemantic.SPEEDS, CommandSemantic.CURRENT_LIMITS], +) +def test_write_chain_publishes_only_complete_feedback(semantic): + caps = get_declared_capabilities("O30", side="right", transport_kind="can") + t = O30ProtocolBackend(HandSide.RIGHT, can_fd=False).command_transaction( + semantic, (17,) * 20, caps + ) + for si in range(0, 25, 5): + m = t.request_messages[0] + assert m.data[1] == si and len(m.data) == 8 + r = t.consume( + can.Message(arbitration_id=0x401, data=m.data, is_extended_id=False) + ) + assert r.complete and r.acknowledged + assert (r.result is not None) == ( + si == 20 and semantic is CommandSemantic.ANGLES + ) + t = t.build_next_transaction() + assert t is None + + +@pytest.mark.parametrize("failure", ["drop_write_si", "error_write_si"]) +def test_middle_stage_failure_stops_sends_and_does_not_publish_partial_vector(failure): + hand, transport = make_hand(**{failure: 10}) + try: + hand.open() + initial = hand.get_snapshot().angles + handle = hand.set_angles([17] * 20) + assert handle.wait(timeout=2) is CommandStage.FAILED + writes = [ + m for m in transport.requests if len(m.data) > 3 and m.data[0] & 0x7F == 1 + ] + assert [m.data[1] for m in writes] == [0, 5, 10] + assert hand.get_snapshot().angles.raw == initial.raw + assert handle.error is not None + finally: + hand.close() + + +@pytest.mark.parametrize("sensor", ["NO_SENSOR", "TASHAN_GATHER", "TSSP_JZG"]) +def test_classic_lifecycle_whole_hand_write_and_sensor_fragments(sensor): + hand, _transport = make_hand(O30DeviceScript(sensor_variant=sensor)) + try: + hand.open() + assert hand.get_angles(timeout=1).raw == tuple(range(20)) + assert hand.set_angles([17] * 20).wait(timeout=2) is CommandStage.ACKNOWLEDGED + assert hand.get_angles(timeout=1).raw == (17,) * 20 + assert hand.set_speeds([40] * 20).wait(timeout=2) is CommandStage.ACKNOWLEDGED + assert hand.get_configured_speeds(timeout=1).raw == (40,) * 20 + if sensor == "NO_SENSOR": + with pytest.raises(UnsupportedFirmwareError): + hand.get_force(timeout=2) + else: + force = hand.get_force("fn", timeout=3) + assert force.fn is not None + assert len(force.fn.raw) == 5 + assert len(force.fn.raw[1]) == (1 if sensor == "TASHAN_GATHER" else 70) + hand.close() + hand.open() + assert hand.get_angles(timeout=1).raw == (17,) * 20 + finally: + hand.close() + + +def test_classic_absent_first_and_last_fingers_do_not_hide_installed_sensors(): + from .test_o30_tactile_availability import MixedSensors + + script = MixedSensors(sensor_variant="TSSP_JZG") + script.absent = {1, 5} + script.selected_sensor = 5 + hand, _ = make_hand(script) + try: + hand.open() + force = hand.get_force("fn", timeout=3) + assert force.fn.raw[0] is None and force.fn.raw[4] is None + assert len(force.fn.raw[1]) == 70 + finally: + hand.close() + + +def test_unrelated_fault_is_not_proof_that_tactile_is_unsupported(): + class NoSelector(O30DeviceScript): + def _write(self, main_index, sub_index, length, payload, *, rts=False): + if main_index == 0x31 and sub_index == 0x6E: + return (self._response(0x0C, 0, bytes(5)),) + return super()._write(main_index, sub_index, length, payload, rts=rts) + + hand, _ = make_hand(NoSelector(application_version="1.0.0")) + try: + hand.open() + from linkerbot_hand import TimeoutError + + with pytest.raises(TimeoutError): + hand.get_force(timeout=2) + assert hand.get_capabilities().telemetry_supported(TelemetrySemantic.FORCE) + assert hand.get_health().connected + assert hand.get_angles(timeout=1).raw == tuple(range(20)) + finally: + hand.close() + + +def test_classic_no_sensor_repeated_type_reply_is_unsupported(): + class RepeatType(O30DeviceScript): + def _write(self, main_index, sub_index, length, payload, *, rts=False): + if main_index == 0x31 and sub_index == 0x6E: + return (self._response(0x31, 0, self._sensor_object(1)[:32]),) + return super()._write(main_index, sub_index, length, payload, rts=rts) + + hand, _ = make_hand(RepeatType(application_version="1.0.0")) + try: + hand.open() + with pytest.raises(UnsupportedFirmwareError): + hand.get_force(timeout=2) + assert hand.get_health().connected + finally: + hand.close() + + +@pytest.mark.parametrize("firmware", ["1.0.0", "1.2.0"]) +def test_late_fault_does_not_hide_four_installed_fingertips(firmware): + from .test_o30_tactile_availability import MixedSensors + + class LateFault(MixedSensors): + absent = frozenset({1}) + + def _write(self, main_index, sub_index, length, payload, *, rts=False): + replies = super()._write(main_index, sub_index, length, payload, rts=rts) + if main_index == 0x31 and sub_index == 0x6E and payload == b"\x01": + return (self._response(0x0C, 20, bytes(5)),) + replies + return replies + + hand, _ = make_hand( + LateFault(sensor_variant="TSSP_JZG", application_version=firmware) + ) + try: + hand.open() + force = hand.get_force("fn", timeout=2) + assert force.fn.raw[0] is None + assert all(len(values) == 70 for values in force.fn.raw[1:]) + assert hand.get_capabilities().telemetry_supported(TelemetrySemantic.FORCE) + finally: + hand.close() + + +def test_serialized_reads_support_firmware_without_burst_reply_capacity(): + class NoBurstReplies(O30DeviceScript): + def _read(self, main_index, sub_index, length, *, rts): + if length > 5: + return () + return super()._read(main_index, sub_index, length, rts=rts) + + hand, transport = make_hand(NoBurstReplies(sensor_variant="TASHAN_GATHER")) + try: + hand.open() + assert hand.get_angles(timeout=1).raw == tuple(range(20)) + assert hand.get_force("fn", timeout=3).fn is not None + requests = [m for m in transport.requests if len(m.data) == 3] + assert requests and all(1 <= m.data[2] <= 5 for m in requests) + finally: + hand.close() + + +def test_serialized_read_ignores_future_ranges_and_publishes_only_full_vector(): + caps = get_declared_capabilities("O30", side="right", transport_kind="can") + tx = O30ProtocolBackend(HandSide.RIGHT, can_fd=False).read_transaction( + TelemetrySemantic.ANGLES, caps + ) + wire = encode_v25(tuple(range(20))) + + def frame(start, payload): + return can.Message( + arbitration_id=0x401, + data=bytes((1, start, len(payload))) + payload, + is_extended_id=False, + ) + + assert not tx.consume(frame(20, wire[20:25])).matched + for start in range(0, 25, 5): + assert bytes(tx.request_messages[0].data) == bytes((1, start, 5)) + # One requested range can itself arrive in reordered small fragments. + partial = tx.consume(frame(start + 2, wire[start + 2 : start + 5])) + assert partial.matched and not partial.complete and partial.result is None + assert not tx.consume(frame(start, wire[start : start + 1])).complete + progress = tx.consume(frame(start + 1, wire[start + 1 : start + 2])) + assert progress.complete + if start == 20: + assert progress.result.raw == tuple(range(20)) + else: + assert progress.result is None + tx = tx.build_next_transaction() + assert tx is None + + +def test_missing_middle_read_reply_stops_later_ranges_and_keeps_snapshot(): + class MissingRange(O30DeviceScript): + drop = False + + def _read(self, main_index, sub_index, length, *, rts): + if self.drop and main_index == 1 and sub_index == 10: + return () + return super()._read(main_index, sub_index, length, rts=rts) + + script = MissingRange() + hand, transport = make_hand(script) + try: + hand.open() + initial = hand.get_snapshot().angles + transport.requests.clear() + script.drop = True + from linkerbot_hand import TimeoutError + + with pytest.raises(TimeoutError): + hand.get_angles(timeout=1) + assert [m.data[1] for m in transport.requests] == [0, 5, 10] + assert hand.get_snapshot().angles.meta.sequence == initial.meta.sequence + assert hand.get_snapshot().angles.raw == initial.raw + finally: + hand.close() + + +def test_fd_read_retains_single_request(): + caps = get_declared_capabilities("O30", side="right") + tx = O30ProtocolBackend(HandSide.RIGHT).read_transaction( + TelemetrySemantic.ANGLES, caps + ) + assert bytes(tx.request_messages[0].data) == bytes((1, 0, 25)) + assert tx.build_next_transaction() is None diff --git a/tests/hand/api_v1/runtime/test_o30_classic_example.py b/tests/hand/api_v1/runtime/test_o30_classic_example.py new file mode 100644 index 0000000..0892b37 --- /dev/null +++ b/tests/hand/api_v1/runtime/test_o30_classic_example.py @@ -0,0 +1,365 @@ +"""Exercise preserves target registers and records independent physical feedback.""" + +from argparse import Namespace +from dataclasses import replace + +import can +import pytest + +from examples.o30_classic_can import exercise, read_targets +from linkerbot_hand.protocols.o30.codec import decode_v25, encode_v25 + +from .o30_canfd_fake import O30DeviceScript +from .test_o30_classic_can import make_hand + + +def test_polling_with_new_samples_does_not_hide_a_query_timeout(monkeypatch): + class DropOnce(O30DeviceScript): + polling = False + angle_ranges = 0 + + def _read(self, main_index, sub_index, length, *, rts): + if self.polling and main_index == 1 and sub_index == 10: + self.angle_ranges += 1 + if self.angle_ranges == 2: + return () + return super()._read(main_index, sub_index, length, rts=rts) + + script = DropOnce() + hand, _ = make_hand(script) + report = {} + start_polling = hand.start_polling + + def start(plan): + script.polling = True + return start_polling(plan) + + monkeypatch.setattr(hand, "start_polling", start) + try: + hand.open() + with pytest.raises( + RuntimeError, match="polling reported a communication error" + ): + exercise(hand, options(duration=0.3), report) + assert report["polling"]["observed_unique_angle_samples"] > 0 + assert report["health"]["connected"] + assert report["health"]["recent_error"]["code"] == "timeout" + finally: + hand.close() + + +def options(**overrides): + return Namespace( + **( + { + "write_back": False, + "motion_joint": None, + "delta": 2, + "angles_hz": 100, + "duration": 0.04, + "motion_timeout": 0.04, + } + | overrides + ) + ) + + +class NoMotion(O30DeviceScript): + def _write(self, main_index, sub_index, length, payload, *, rts=False): + before = self.actual[1] + replies = super()._write(main_index, sub_index, length, payload, rts=rts) + if main_index == 1: + self.actual[1] = before + return replies + + +@pytest.mark.parametrize("write_back", [False, True]) +def test_read_and_configuration_exercise_never_writes_angles(write_back): + script = O30DeviceScript() + script.targets[1] = encode_v25((100,) * 20) + hand, transport = make_hand(script) + report = {} + try: + hand.open() + exercise(hand, options(write_back=write_back), report) + assert report["force"]["status"] == "SKIP" + assert report["polling"]["observed_unique_angle_samples"] > 0 + assert decode_v25(script.targets[1]) == (100,) * 20 + assert not any( + m.data[0] & 0x7F == 1 and len(m.data) > 3 for m in transport.requests + ) + if write_back: + assert report["write_back"]["configured_speeds"]["readback"] == list( + range(30, 50) + ) + finally: + hand.close() + + +def test_successful_motion_restores_original_targets_and_actual_angles(): + hand, transport = make_hand() + original = decode_v25(transport.script.targets[1]) + report = {} + try: + hand.open() + exercise( + hand, + options(motion_joint=6), + report, + target_reader=lambda: decode_v25(transport.script.targets[1]), + ) + motion = report["motion"] + assert motion["status"] == "TARGETS_VERIFIED" + assert motion["actual_motion_observed"] + assert motion["signed_motion_raw"] == 2 + assert motion["restore_error_raw"] == 0 + assert motion["restored_targets"] == original + assert motion["other_joint_max_change_raw"] == 0 + finally: + hand.close() + + +@pytest.mark.parametrize("delta", [2, -2, 1]) +def test_no_actual_movement_is_recorded_without_failing_protocol_validation(delta): + hand, transport = make_hand(NoMotion()) + report = {} + try: + hand.open() + exercise( + hand, + options(motion_joint=6, delta=delta), + report, + target_reader=lambda: decode_v25(transport.script.targets[1]), + ) + assert report["motion"]["command"]["stage"] == "acknowledged" + assert report["motion"]["status"] == "TARGETS_VERIFIED" + assert not report["motion"]["actual_motion_observed"] + assert report["motion"]["signed_motion_raw"] == 0 + assert report["motion"]["target_error_raw"] == abs(delta) + assert report["motion"]["restore"]["stage"] == "acknowledged" + finally: + hand.close() + + +def test_single_joint_exercise_preserves_other_unreached_targets(): + script = NoMotion() + original = [100] * 20 + original[6] = 6 + script.targets[1] = encode_v25(tuple(original)) + hand, _ = make_hand(script) + report = {} + try: + hand.open() + exercise( + hand, + options(motion_joint=6), + report, + target_reader=lambda: decode_v25(script.targets[1]), + ) + expected = original.copy() + expected[6] += 2 + assert report["motion"]["target_readback"] == tuple(expected) + assert decode_v25(script.targets[1]) == tuple(original) + finally: + hand.close() + + +def test_actual_target_offset_does_not_replace_original_targets_or_fail_validation(): + script = O30DeviceScript() + script.targets[1] = encode_v25((100,) * 20) + hand, _ = make_hand(script) + report = {} + try: + hand.open() + exercise( + hand, + options(motion_joint=6), + report, + target_reader=lambda: decode_v25(script.targets[1]), + ) + motion = report["motion"] + assert motion["status"] == "TARGETS_VERIFIED" + assert motion["initial_actual"][6] == 6 + assert motion["target_readback"][6] == 102 + assert motion["restored_targets"] == (100,) * 20 + assert decode_v25(script.targets[1]) == (100,) * 20 + finally: + hand.close() + + +def test_target_readback_failure_still_restores_original_targets(): + hand, transport = make_hand() + original = decode_v25(transport.script.targets[1]) + report = {} + calls = 0 + + def targets(): + nonlocal calls + calls += 1 + return (0,) * 20 if calls == 2 else decode_v25(transport.script.targets[1]) + + try: + hand.open() + with pytest.raises(RuntimeError, match="readback mismatch"): + exercise(hand, options(motion_joint=6), report, target_reader=targets) + assert report["motion"]["restored_targets"] == original + assert decode_v25(transport.script.targets[1]) == original + finally: + hand.close() + + +def test_feedback_query_failure_still_restores_targets_and_fails_validation(): + hand, transport = make_hand() + original = decode_v25(transport.script.targets[1]) + original_read = hand.get_angles + calls = 0 + report = {} + + def angles(timeout): + nonlocal calls + calls += 1 + if calls == 3: + raise TimeoutError("feedback query failed") + return original_read(timeout=timeout) + + try: + hand.open() + hand.get_angles = angles + with pytest.raises(TimeoutError, match="feedback query failed"): + exercise( + hand, + options(motion_joint=6), + report, + target_reader=lambda: decode_v25(transport.script.targets[1]), + ) + assert report["motion"]["restored_targets"] == original + assert "command_error" in report["motion"] + assert "status" not in report["motion"] + assert decode_v25(transport.script.targets[1]) == original + finally: + hand.close() + + +def test_restore_readback_mismatch_is_an_error_even_with_physical_offsets_allowed(): + hand, transport = make_hand() + report = {} + calls = 0 + + def targets(): + nonlocal calls + calls += 1 + return (0,) * 20 if calls == 3 else decode_v25(transport.script.targets[1]) + + try: + hand.open() + with pytest.raises(RuntimeError, match="target register restoration failed"): + exercise(hand, options(motion_joint=6), report, target_reader=targets) + assert "restore_error" in report["motion"] + assert "status" not in report["motion"] + finally: + hand.close() + + +def test_read_targets_assembles_only_rts_target_fragments(monkeypatch): + wire = encode_v25(tuple(range(20))) + messages = [] + + class Bus: + def __enter__(self): + return self + + def __exit__(self, *args): + pass + + def set_filters(self, filters): + pass + + def send(self, message, timeout): + start = message.data[1] + assert bytes(message.data) == bytes((0x81, start, 5)) + assert not message.is_fd + messages.extend( + [ + can.Message( + arbitration_id=0x401, + data=b"\x01\x00\x01\xff", + is_extended_id=False, + ), + can.Message( + arbitration_id=0x401, + data=bytes((0x81, start, 5)) + wire[start : start + 5], + is_extended_id=False, + ), + ] + ) + + def recv(self, timeout): + return messages.pop(0) if messages else None + + monkeypatch.setattr("examples.o30_classic_can.can.Bus", lambda **kwargs: Bus()) + assert read_targets("unused", 1) == tuple(range(20)) + + +def test_other_joint_feedback_change_is_recorded_without_changing_targets(): + hand, transport = make_hand() + report = {} + original_read = hand.get_angles + calls = 0 + + def angles(timeout): + nonlocal calls + calls += 1 + sample = original_read(timeout=timeout) + if calls == 3: + values = list(sample.raw) + values[5] += 10 + return replace(sample, raw=tuple(values)) + return sample + + try: + hand.open() + hand.get_angles = angles + exercise( + hand, + options(motion_joint=6), + report, + target_reader=lambda: decode_v25(transport.script.targets[1]), + ) + assert report["motion"]["status"] == "TARGETS_VERIFIED" + assert report["motion"]["other_joint_max_change_raw"] == 10 + finally: + hand.close() + + +def test_transient_feedback_is_recorded_but_not_claimed_as_arrival(): + hand, transport = make_hand(NoMotion()) + report = {} + original_read = hand.get_angles + calls = 0 + + def angles(timeout): + nonlocal calls + calls += 1 + sample = original_read(timeout=timeout) + if calls == 3: + values = list(sample.raw) + values[6] += 2 + return replace(sample, raw=tuple(values)) + return sample + + try: + hand.open() + hand.get_angles = angles + exercise( + hand, + options(motion_joint=6), + report, + target_reader=lambda: decode_v25(transport.script.targets[1]), + ) + assert report["motion"]["status"] == "TARGETS_VERIFIED" + assert report["motion"]["actual_motion_observed"] + assert report["motion"]["observed"][6] == 6 + assert report["motion"]["target_error_raw"] == 2 + assert "arrival_confirmed" not in report["motion"] + finally: + hand.close() diff --git a/tests/hand/api_v1/runtime/test_o6_can_mode_script.py b/tests/hand/api_v1/runtime/test_o6_can_mode_script.py new file mode 100644 index 0000000..166c64e --- /dev/null +++ b/tests/hand/api_v1/runtime/test_o6_can_mode_script.py @@ -0,0 +1,118 @@ +import importlib.util +from pathlib import Path + +import can +import pytest + +SCRIPT = Path(__file__).resolve().parents[4] / "examples" / "o6_can_mode.py" +spec = importlib.util.spec_from_file_location("o6_can_mode", SCRIPT) +utility = importlib.util.module_from_spec(spec) +spec.loader.exec_module(utility) + + +class ModeBus: + def __init__( + self, fd=True, *, model=b"LHO6\x00", side=b"LEFT\x00", apply=True, brs=0 + ): + self.fd = fd + self.model = model + self.side = side + self.apply = apply + self.brs = brs + self.sent = [] + self.pending = [] + self.drop_save_reply = False + + def send(self, message, timeout): + self.sent.append(message) + if message.is_fd != self.fd: + return + data = bytes(message.data) + if data[:3] == b"\x35\x06\x01": + if self.apply: + self.fd = data[3] == 2 + return + values = { + bytes.fromhex("410005"): self.model, + bytes.fromhex("41F105"): self.side, + bytes.fromhex("B50604"): bytes((2 if self.fd else 1, self.brs, 1, 2)), + bytes.fromhex("351E0101"): b"\x00", + } + if data == bytes.fromhex("351E0101") and self.drop_save_reply: + return + self.pending.append( + can.Message( + arbitration_id=message.arbitration_id | 0x400, + data=data[:3] + values[data], + is_extended_id=False, + is_fd=self.fd, + ) + ) + + def recv(self, timeout): + return self.pending.pop(0) if self.pending else None + + +@pytest.mark.parametrize("source_fd,target_fd", [(True, False), (False, True)]) +def test_bidirectional_switch_with_no_default_flash_save(source_fd, target_fd): + bus = ModeBus(source_fd) + utility.switch(bus, 0x123, source_fd, target_fd, "left", timeout=0.1) + writes = [message for message in bus.sent if len(message.data) > 3] + assert len(writes) == 1 + assert writes[0].is_fd is source_fd + assert bytes(writes[0].data) == bytes((0x35, 6, 1, 2 if target_fd else 1)) + assert bus.fd is target_fd + + +@pytest.mark.parametrize("options", [{"model": b"O30\x00\x00"}, {"side": b"RIGHT"}]) +def test_identity_mismatch_prevents_configuration_write(options): + bus = ModeBus(**options) + with pytest.raises(RuntimeError): + utility.switch(bus, 1, True, False, "left", timeout=0.1) + assert all(len(message.data) == 3 for message in bus.sent) + + +def test_brs_must_be_disabled_before_classic_switch(): + bus = ModeBus(brs=1) + with pytest.raises(RuntimeError, match="BRS"): + utility.switch(bus, 1, True, False, "left", timeout=0.1) + assert all(len(message.data) == 3 for message in bus.sent) + + +def test_ignored_switch_does_not_retry_or_save(): + bus = ModeBus(apply=False) + with pytest.raises(TimeoutError): + utility.switch(bus, 1, True, False, "left", timeout=0.1, save=True) + assert len([message for message in bus.sent if len(message.data) > 3]) == 1 + + +def test_explicit_save_is_dispatched_once_and_missing_reply_is_failure(): + bus = ModeBus() + bus.drop_save_reply = True + with pytest.raises(TimeoutError): + utility.switch(bus, 1, True, False, "left", timeout=0.1, save=True) + saves = [ + message + for message in bus.sent + if bytes(message.data) == bytes.fromhex("351E0101") + ] + assert len(saves) == 1 and not saves[0].is_fd + + +def test_check_only_reads(): + bus = ModeBus() + utility.inspect(bus, 1, True, "left", 0.1) + assert all(len(message.data) == 3 for message in bus.sent) + + +def test_successful_save_and_already_selected_mode(): + bus = ModeBus(False) + utility.switch(bus, 1, False, False, "left", timeout=0.1, save=True) + writes = [message for message in bus.sent if len(message.data) > 3] + assert [bytes(message.data) for message in writes] == [bytes.fromhex("351E0101")] + + +@pytest.mark.parametrize("value", ["nan", "inf", "0", "-1"]) +def test_invalid_timeout_does_not_touch_host(value): + with pytest.raises(SystemExit): + utility.main(["--side", "left", "--from-mode", "canfd", "--timeout", value]) diff --git a/tests/hand/api_v1/runtime/test_o6_classic_can.py b/tests/hand/api_v1/runtime/test_o6_classic_can.py new file mode 100644 index 0000000..661f093 --- /dev/null +++ b/tests/hand/api_v1/runtime/test_o6_classic_can.py @@ -0,0 +1,136 @@ +from contextlib import closing +from dataclasses import replace + +import can +import pytest + +from linkerbot_hand import ( + DeviceConfig, + HandDevice, + HandSide, + ProtocolKind, + TimeoutError, + TransportKind, + discover_hands, +) +from linkerbot_hand.api.commands import CommandStage +from linkerbot_transport import CANRouteKey +from linkerbot_transport.canfd import CANFDMessage +from linkerbot_transport.testing import FakeTransport +from tests.hand.api_v1.discovery.test_discovery import _FAST, _classic_channel, _Source +from tests.hand.api_v1.runtime.hop_six_fake import HOPSixScript + + +class ClassicSixTransport(FakeTransport): + def __init__(self, script): + super().__init__( + route_key_of=lambda message: CANRouteKey( + message.arbitration_id, message.is_extended_id + ) + ) + self.script = script + self.requests = [] + + def send(self, message): + assert isinstance(message, can.Message) + assert not message.is_fd and not message.bitrate_switch + assert len(message.data) <= 8 + receipt = super().send(message) + if receipt.exception() is not None: + return receipt + self.requests.append(message) + for reply in self.script.respond( + CANFDMessage( + arbitration_id=message.arbitration_id, + data=bytes(message.data), + is_extended_id=False, + frame_type=4, + ) + ): + control, start, size = reply.data[:3] + payload = reply.data[3 : 3 + size] + for offset in range(0, len(payload), 5): + chunk = payload[offset : offset + 5] + self.emit( + can.Message( + arbitration_id=reply.arbitration_id, + data=bytes((control, start + offset, len(chunk))) + chunk, + is_extended_id=False, + ) + ) + return receipt + + +def make_hand(side=HandSide.LEFT, request_id=1, sensor_variant="NO_SENSOR"): + script = HOPSixScript("O6", side, request_id, sensor_variant) + script.configuration[6] = 1 + transport = ClassicSixTransport(script) + hand = HandDevice( + model="O6", + side=side, + protocol="hop", + transport=transport, + config=DeviceConfig(hop_transport=TransportKind.CAN, hop_request_id=request_id), + ) + return script, transport, hand + + +@pytest.mark.parametrize("side", [HandSide.LEFT, HandSide.RIGHT]) +@pytest.mark.parametrize("request_id", [1, 0x123]) +def test_classic_identity_control_and_reopen(side, request_id): + script, transport, hand = make_hand(side, request_id) + with closing(hand.open()): + assert hand.get_device_info(timeout=1).reported_side is side + assert hand.get_capabilities().protocol is ProtocolKind.HOP + assert hand.get_capabilities().transport.kind is TransportKind.CAN + assert hand.get_capabilities().transport.data_bitrate is None + before = script.targets[1] + command = hand.set_angles([100] * 6) + command.wait(1) + assert command.stage is CommandStage.ACKNOWLEDGED + assert hand.get_angles(timeout=1).raw == (100,) * 6 + assert script.targets[1][6:10] == before[6:10] + assert hand.get_can_configuration().interface_type == 1 + assert all( + message.arbitration_id == request_id for message in transport.requests + ) + with closing(hand.open()): + assert hand.get_angles(timeout=1).raw == (100,) * 6 + + +def test_classic_tactile_long_reads(): + _, transport, hand = make_hand(sensor_variant="TSSP_JZG") + with closing(hand.open()): + sample = hand.get_normal_force_resultant(timeout=2) + assert sample.raw == tuple(1000 + index for index in range(1, 6)) + assert all(message.data[2] <= 5 for message in transport.requests) + + +def test_incomplete_second_range_is_not_acknowledged(): + script, transport, hand = make_hand() + with closing(hand.open()): + previous = hand.get_angles(timeout=1) + script.drop_range = (1, 10) + command = hand.set_angles([100] * 6) + command.wait(1) + assert command.stage is not CommandStage.ACKNOWLEDGED + assert hand.get_snapshot().angles.raw == previous.raw + writes = [message for message in transport.requests if len(message.data) > 3] + assert len(writes) == 2 + with pytest.raises(TimeoutError): + hand.get_angles(timeout=0.2) + + +@pytest.mark.parametrize("side", [HandSide.LEFT, HandSide.RIGHT]) +def test_classic_discovery_preserves_mode_and_endpoint(side): + script = HOPSixScript("O6", side, 0x123) + source = _Source( + _classic_channel(), + {TransportKind.CAN: lambda: ClassicSixTransport(script)}, + ) + report = discover_hands(replace(_FAST, hop_request_ids=(0x123,)), source=source) + candidate = report.require_one() + assert candidate.model == "O6" and candidate.side is side + assert candidate.wire_protocol is TransportKind.CAN + with closing(candidate.create_hand_device().open()) as hand: + assert hand.get_angles(timeout=1).raw == (10, 5, 11, 12, 13, 14) diff --git a/tests/hand/api_v1/runtime/test_transactions.py b/tests/hand/api_v1/runtime/test_transactions.py index dcbb19d..7405749 100644 --- a/tests/hand/api_v1/runtime/test_transactions.py +++ b/tests/hand/api_v1/runtime/test_transactions.py @@ -239,3 +239,85 @@ def test_pending_waiter_cancel_removes_unused_transaction() -> None: assert coordinator.pending_count == 0 coordinator.reject_new(ClosedError("closed")) + + +def test_exclusive_request_replaces_full_queue_and_ignores_old_completion() -> None: + coordinator, _clock, wake = make_coordinator(capacity=2) + coordinator.activate() + inflight = coordinator.request( + TelemetrySemantic.ANGLES, start_sequence=0, deadline_s=1 + ) + assert coordinator.take_next() is not None + queued = coordinator.request( + TelemetrySemantic.CURRENTS, start_sequence=0, deadline_s=1 + ) + assert coordinator.request_poll(TelemetrySemantic.TEMPERATURES) + wake.clear() + error = DeviceNotReadyError("exclusive configuration") + + exclusive = coordinator.request_exclusive( + "configuration", start_sequence=0, deadline_s=1, error=error + ) + + assert wake.is_set() + assert coordinator.pending_count == coordinator.waiter_count == 1 + for old in (inflight, queued): + with pytest.raises(DeviceNotReadyError) as raised: + old.wait() + assert raised.value is error + with pytest.raises(DeviceNotReadyError) as raised: + coordinator.request(TelemetrySemantic.ANGLES, start_sequence=0, deadline_s=1) + assert raised.value is error + assert not coordinator.request_poll(TelemetrySemantic.ANGLES) + with pytest.raises(DeviceNotReadyError): + coordinator.request_exclusive( + "another_configuration", start_sequence=0, deadline_s=1, error=error + ) + coordinator.cancel(inflight) + coordinator.complete(TelemetrySemantic.ANGLES, "old sample", sequence=1) + assert coordinator.pending_count == coordinator.waiter_count == 1 + claim = coordinator.take_next() + assert claim is not None and claim.key == "configuration" + assert coordinator.take_next() is None + coordinator.complete("configuration", True, sequence=1) + assert exclusive.wait() is True + assert coordinator.pending_count == coordinator.waiter_count == 0 + # A successful write does not reopen admission. Only initialization does. + with pytest.raises(DeviceNotReadyError): + coordinator.request(TelemetrySemantic.ANGLES, start_sequence=0, deadline_s=1) + coordinator.activate() + assert coordinator.request_poll(TelemetrySemantic.ANGLES) + + +def test_expired_exclusive_request_preserves_old_work_and_admission() -> None: + coordinator, clock, _wake = make_coordinator() + coordinator.activate() + old = coordinator.request(TelemetrySemantic.ANGLES, start_sequence=0, deadline_s=2) + clock.advance(1) + with pytest.raises(TimeoutError, match="deadline has already expired"): + coordinator.request_exclusive( + "configuration", + start_sequence=0, + deadline_s=1, + error=DeviceNotReadyError("exclusive configuration"), + ) + assert coordinator.pending_count == coordinator.waiter_count == 1 + assert coordinator.request_poll(TelemetrySemantic.CURRENTS) + coordinator.complete(TelemetrySemantic.ANGLES, "old sample", sequence=1) + assert old.wait() == "old sample" + + +def test_exclusive_request_does_not_reopen_closed_admission() -> None: + coordinator, _clock, _wake = make_coordinator() + coordinator.activate() + closed = ClosedError("session closed") + coordinator.reject_new(closed) + with pytest.raises(ClosedError) as raised: + coordinator.request_exclusive( + "configuration", + start_sequence=0, + deadline_s=1, + error=DeviceNotReadyError("exclusive configuration"), + ) + assert raised.value is closed + assert coordinator.pending_count == coordinator.waiter_count == 0 diff --git a/tests/hand/api_v1/test_hand_device_contract.py b/tests/hand/api_v1/test_hand_device_contract.py index 318791f..b41ed57 100644 --- a/tests/hand/api_v1/test_hand_device_contract.py +++ b/tests/hand/api_v1/test_hand_device_contract.py @@ -73,6 +73,10 @@ "set_motion_times", "get_motion_times", "get_device_info", + "get_can_configuration", + "set_can_id", + "set_can_brs", + "set_can_bitrates", "set_stall_detection_times", "get_stall_detection_times", "set_stall_thresholds",