From 9b29ac78042192587a1657e80d3ae58818d4e5ec Mon Sep 17 00:00:00 2001 From: AlexTemirov Date: Sun, 13 Sep 2026 15:49:40 -0700 Subject: [PATCH] Add per-servo actuator setup and saved calibration tests --- README.md | 52 +++ blacknode-package.toml | 4 +- nodes/__init__.py | 1 + nodes/actuator_setup.py | 265 ++++++++++++ nodes/actuator_states.py | 384 ++++++++++++++++++ nodes/calibration_control.py | 7 + pyproject.toml | 2 +- templates/actuator-setup.json | 26 ++ tests/test_actuator_states.py | 320 +++++++++++++++ tests/test_robot_actuator_setup.py | 187 +++++++++ tests/test_robot_template_device_selection.py | 1 + 11 files changed, 1246 insertions(+), 3 deletions(-) create mode 100644 nodes/actuator_setup.py create mode 100644 nodes/actuator_states.py create mode 100644 templates/actuator-setup.json create mode 100644 tests/test_actuator_states.py create mode 100644 tests/test_robot_actuator_setup.py diff --git a/README.md b/README.md index ffff3f7..99a22ee 100644 --- a/README.md +++ b/README.md @@ -30,6 +30,58 @@ Core nodes include `Robot`, `ComputeDevice`, `PhysicalRobot`, `RobotDeployment`, The `Robot` calibration input also accepts Blacknode's explicit calibration-import envelope. SO-ARM motor range JSON selected through an operator app is validated against the profile, bound to the discovered USB serial, converted to Blacknode's native degree-and-safety format, and copied into the persistent Blacknode directory. Runtime use has no dependency on the tool that originally produced the JSON. +## Actuator Setup workflow + +Press the first shelf button, **Actuator Setup**, select a USB port and press +**Scan**. A separate connected node appears for each responding servo, showing its +ID, model, raw position, voltage, temperature, torque, hardware warnings and +read-only hardware settings. Rescanning updates existing cards and flags missing +IDs. Choose another baud rate under **Advanced** if no actuator responds. The scan +covers IDs 0–253 in one bounded operation. Discovery needs no robot profile. + +For assignment, support the arm, power off, connect only the new actuator and +scan again after powering on. Enter **New ID** (1–253) on that servo's card. +Keep that actuator supported and press **Set ID**. The button performs a fresh +scan, programs the ID and verifies the result. The ID controls have no confirmation +checkboxes or manual scan-expiry step; changing the ID retires saved calibration. +Legacy clients retain their expiring, single-use confirmation token contract. +Programming requires torque off and a supported, warning-free actuator. +If torque is on or unknown, confirm the isolated actuator is supported against +gravity and use **Release torque**, then scan again. + +Saved calibrations for this USB hardware identity are preserved under each +profile's `calibrations/retired/` directory before an assignment attempt. They +are excluded from automatic calibration selection, including after an uncertain +write result. Power off and reconnect the complete arm, rescan, and resolve +missing IDs. Expand **Calibration and motion test**, select a robot profile whose +joint IDs match the finished assembly, and press **Calibrate**. Record new +hand-guided calibration, then **Open motion test** for the selected servo's +explicitly armed testing within calibrated limits. + +The node's ordinary cook is inert. Operator controls delegate through the +USB-matched `_bn_robot_actuator_setup_provider` contract: `scan(config)` +returns `actuators`; `assign(config, expected, new_id)` verifies the physical +change and returns `assigned`, `old_id`, `new_id`, `actuators` and `report`. +`release(config, expected)` independently verifies torque off and returns +`released`, `actuators` and `report`. +The calibration panel uses optional `read_position(config, servo_id)` feedback +to follow hand movement in raw ticks before arming. Each capture reads fresh +feedback; incomplete points remain drafts across reloads. **Arm** validates and +saves a complete range, with **Save states** also available separately. +Protocol handling stays in `blacknode-drivers`. A mock provider supplies the +same contract for hardware-free development. Current physical ID programming +supports STS3215 through a Local USB adapter; managed-device setup is not exposed. + +Each card also captures released **Min / Max** endpoints, optional **Home**, and +named measured poses. Either endpoint tick direction is accepted; an interior +Home or a calculated midpoint supplies the test origin while original captures +are preserved. **Arm** checks and saves the range before starting the +test slider, with exact captured endpoint limits and direct position targets. Capable +providers enforce finite speed and acceleration in their position controller. The +managed test releases torque on lost controls, invalid feedback or Stop. Saved +points and poses follow the physical USB identity, provider and servo ID and are +retired on ID changes. Whole-robot calibration remains separately available. + ## Safety - Motion is disarmed by default. diff --git a/blacknode-package.toml b/blacknode-package.toml index a010507..591d600 100644 --- a/blacknode-package.toml +++ b/blacknode-package.toml @@ -1,6 +1,6 @@ [package] name = "blacknode-robot" -version = "0.5.7" +version = "0.5.8" description = "Robot contracts, connected-device lifecycle, normalized telemetry, profiles, and driver launch." requires-blacknode = ">=0.3.0" layer = "robot" @@ -69,7 +69,7 @@ node-types = [ "DeviceInspect", "RobotCapabilityInspect", "RobotCapabilityList", "RobotCapabilityProfile", "RobotConnectionDashboard", "RobotDiscovery", "RobotMonitor", - "RobotRawMonitor", "RobotRawMonitorMockProvider", + "RobotRawMonitor", "RobotRawMonitorMockProvider", "ActuatorSetup", "ActuatorServoSetup", "RobotActuatorSetupMockProvider", "RobotROSCapabilityDiscover", "RobotROSInterfaceCheck", "RobotUSBDiscovery" ] diff --git a/nodes/__init__.py b/nodes/__init__.py index 2e24020..a44f095 100644 --- a/nodes/__init__.py +++ b/nodes/__init__.py @@ -5,3 +5,4 @@ from . import monitoring # noqa: F401 from . import presets # noqa: F401 from . import robot # noqa: F401 +from . import actuator_setup # noqa: F401 diff --git a/nodes/actuator_setup.py b/nodes/actuator_setup.py new file mode 100644 index 0000000..eff0f15 --- /dev/null +++ b/nodes/actuator_setup.py @@ -0,0 +1,265 @@ +"""Operator-controlled USB actuator setup over replaceable provider contracts.""" +from __future__ import annotations + +import secrets +import copy +import threading +import time +from collections.abc import Mapping + +from blacknode.node import Bool, Dict, Int, List, Text, node, _NODE_REGISTRY +from .calibration_control import _provider_binding +from . import profiles + +_tickets = {} +_lock = threading.Lock() +_TTL = 120.0 + + +def _resolve(ctx): + profile_id = str(ctx.get("profile_id") or "").strip() + port = str(ctx.get("serial_port") or "").strip() + if not port: + raise ValueError("Pick a USB port, then press Scan") + providers = [value for fn in _NODE_REGISTRY.values() + if isinstance(value := getattr(fn, "_bn_robot_actuator_setup_provider", None), Mapping)] + profile = {} + if profile_id and profile_id not in {"auto", "none"}: + profile, _ = profiles.load_profile(profile_id) + if not profile: + raise ValueError("Selected robot profile is unavailable") + errors = profiles._validate_profile(profile) + if errors: + raise ValueError("Invalid robot profile: " + "; ".join(errors)) + profile = profiles._profile_with_default_capabilities(profile, profile.get("driver") or {}) + binding = _provider_binding(profile) + candidates = [p for p in providers if p.get("package") == binding["package"] + and p.get("component") == binding["component"]] + else: + ranked = [(int(p["match_hardware"]({"port": port})), p) for p in providers + if callable(p.get("match_hardware"))] + best = max((score for score, _ in ranked), default=0) + candidates = [p for score, p in ranked if score == best and score > 0] + if len(candidates) != 1: + raise ValueError("Actuator setup provider unavailable or ambiguous for this USB adapter") + provider = candidates[0] + if not provider or not callable(provider.get("scan")) or not callable(provider.get("assign")): + raise ValueError("Actuator setup provider unavailable") + return profile, provider, {"port": port, "baudrate": int(ctx.get("baudrate") or 1000000)} + + +def _retire_calibrations(hardware_id): + """Preserve old files for review while preventing automatic reuse on this assembly.""" + retired = [] + for item in profiles.list_profiles(): + while (path := profiles._find_calibration_path(item["id"], hardware_id)) is not None: + destination = path.parent / "retired" / f"{time.time_ns()}-{path.name}" + destination.parent.mkdir(parents=True, exist_ok=True) + path.rename(destination) + retired.append(str(destination)) + from . import actuator_states + return retired + actuator_states.retire_hardware(hardware_id) + + +def control_actuator_setup(ctx, action, payload=None): + payload = payload or {} + from . import actuator_states + if action in actuator_states.ACTIONS: + return actuator_states.control(ctx, action, payload) + base = {"ok": False, "assigned": False, "actuators": [], "scan_token": "", + "profile": {}, "bus": {}, "report": "", "retired_calibrations": []} + try: + profile, provider, config = _resolve(ctx) + base["profile"] = profile + base["bus"] = dict(config) + if action == "inspect": + return {**base, "ok": True, "bus": dict(config), "report": "Choose Scan bus to discover IDs 0–253. " + "Stop monitoring, calibration and motion sessions that own this port first."} + if action not in {"scan", "assign", "release"}: + raise ValueError("Actuator Setup supports inspect, scan, release, or assign") + actuator_states.ensure_idle(config["port"]) + if payload.get("confirm_read_only") is not True: + raise ValueError("Confirm the selected port, power and wiring before scanning") + # USB discovery is transport-neutral and does not open the servo bus. + discovery = _NODE_REGISTRY["RobotUSBDiscovery"]({"port_filter": config["port"], "probe_open": False}) + hardware = discovery.get("hardware") or discovery + recommended = hardware.get("recommended") or {} + if str(recommended.get("path") or "") != config["port"]: + raise ValueError("Selected USB port is no longer connected; refresh the port selection") + hardware_id = profiles._hardware_id({"hardware": hardware}) + if not hardware_id: + raise ValueError("A physical USB hardware identity is required") + base["bus"]["hardware_id"] = hardware_id + identity = (profile.get("id", ""), config["port"], config["baudrate"], hardware_id) + if action == "scan": + result = dict(provider["scan"](config)) + rows = result.get("actuators") or [] + token = secrets.token_urlsafe(24) + with _lock: + now = time.monotonic() + for key, ticket in list(_tickets.items()): + if now - ticket["at"] > _TTL or ticket["identity"] == identity: + _tickets.pop(key, None) + _tickets[token] = {"at": now, "identity": identity, "rows": rows} + unreadable = [row["servo_id"] for row in rows if row.get("discovery_status") == "unreadable"] + reported = {row["servo_id"] for row in rows if row.get("discovery_status") != "unreadable"} + missing = [j["servo_id"] for j in profile.get("joints", []) if j["servo_id"] not in reported] + report = (f"Scan complete on {config['port']}: {len(reported)} responding IDs. " + "Open each card to inspect and set up its servo.") + if unreadable: + report += (f" Unreadable replies at IDs {', '.join(map(str, unreadable))}; " + "these addresses have separate unresolved cards. " + "Servos sharing an ID must be connected one at a time to identify them separately.") + if not rows: + report = "No servos responded. Check power and wiring; try another baud rate under Advanced." + return {**base, "ok": bool(rows), "actuators": rows, "scan_token": token, + "scanned_at": time.time(), "bus": {**config, "hardware_id": hardware_id}, + "missing_ids": missing, "report": report} + # The dedicated button is the operator action. Its adjacent instructions + # require an isolated, supported actuator and explain calibration reset. + # Older clients retain their explicit confirmation/token contract. + button_action = payload.get("operator_action") == action and action in {"assign", "release"} + if button_action and ctx.get("servo_id") is None: + raise ValueError("Use Set ID on a discovered servo card") + if not button_action and payload.get("confirm_isolated") is not True: + raise ValueError("Confirm one isolated actuator supported against gravity") + if action == "assign" and not button_action and payload.get("confirm_recalibrate") is not True: + raise ValueError("Confirm recalibration of the changed assembly") + if button_action: + rows = dict(provider["scan"](config)).get("actuators") or [] + ticket = {"at": time.monotonic(), "identity": identity, "rows": rows} + else: + with _lock: + ticket = _tickets.pop(str(payload.get("scan_token") or ""), None) + if not ticket or ticket["identity"] != identity or time.monotonic() - ticket["at"] > _TTL: + raise ValueError("Discovery expired or selection changed; scan the isolated actuator again") + if len(ticket["rows"]) != 1: + raise ValueError("Connect exactly one actuator and scan again before assigning an ID") + if ctx.get("servo_id") is not None and ctx["servo_id"] != ticket["rows"][0]["servo_id"]: + raise ValueError("This servo card does not match the isolated actuator; scan again") + if ticket["rows"][0].get("discovery_status") == "unreadable": + raise ValueError("This address is unresolved. Connect one actuator and scan until its ID and model can be read") + if action == "release": + release = provider.get("release") + if not callable(release): + raise ValueError("This provider does not support isolated torque release") + result = dict(release(config, ticket["rows"][0])) + return {**base, **result, "ok": result.get("released") is True} + joint_id = str(payload.get("joint_id") or "") + joint = next((j for j in profile.get("joints", []) if j.get("id") == joint_id), None) + new_id = payload.get("new_id") + if new_id is None and joint: + new_id = joint["servo_id"] + if isinstance(new_id, bool) or not isinstance(new_id, int) or not 1 <= new_id <= 253: + raise ValueError("Enter a new servo ID from 1 to 253") + row = ticket["rows"][0] + if not row.get("assignment_supported") or row.get("errors") or row.get("hardware_error_flags"): + raise ValueError("ID programming requires a supported actuator with complete, warning-free feedback") + if row.get("torque_enabled") is not False: + raise ValueError("Support this actuator and release torque before setting its ID") + # Retire calibration before a possibly successful physical write. A failed + # acknowledgement must not leave a changed assembly using old limits. + if new_id != ticket["rows"][0]["servo_id"]: + base["retired_calibrations"] = _retire_calibrations(hardware_id) + result = dict(provider["assign"](config, ticket["rows"][0], new_id)) + return {**base, **result, "ok": bool(result.get("assigned")), "profile": profile, + "joint_id": joint_id, "hardware_id": hardware_id} + except Exception as exc: + return {**base, "report": str(exc)} + + +@node(name="ActuatorSetup", component="capabilities", category="Robot", + description="Pick a USB port and scan to create a separate setup card for every responding servo.", + inputs={"profile_id": Text(default=""), "serial_port": Text(default=""), "baudrate": Int(default=1000000)}, + outputs={"ok": Bool, "assigned": Bool, "actuators": List, "profile": Dict, "bus": Dict, "report": Text}, + primary_inputs=[], primary_outputs=["bus", "report"]) +def actuator_setup(ctx): + # Cooking or replaying a saved workflow never scans, writes EEPROM or moves. + return control_actuator_setup(ctx, "inspect") + + +actuator_setup._bn_actuator_setup_control = control_actuator_setup + + +@node(name="ActuatorServoSetup", component="capabilities", category="Robot", + description="Set up one discovered servo: inspect settings, change its ID, release torque, and calibrate.", + inputs={"bus": Dict, "servo_id": Int(default=1), "profile_id": Text(default="")}, + outputs={"report": Text}, primary_inputs=["bus"], primary_outputs=["report"]) +def actuator_servo_setup(ctx): + return {"report": "Press Scan on the USB node to refresh this servo's settings"} + + +_mock_rows = {} + + +def _mock_scan(config): + with _lock: + rows = _mock_rows.setdefault(config["port"], [{ + "servo_id": 1, "reported_id": 1, "model_number": 777, "model": "Mock actuator", + "assignment_supported": True, + "torque_enabled": False, "raw_position": 2048, "voltage_v": 12.0, + "temperature_c": 25, "hardware_error_flags": 0, "hardware_errors": [], "errors": [], + }]) + return {"actuators": copy.deepcopy(rows)} + + +def _mock_assign(config, expected, new_id): + rows = _mock_scan(config)["actuators"] + if len(rows) != 1 or rows[0] != expected: + raise ValueError("Mock actuator changed; scan again") + if not 1 <= new_id <= 253 or rows[0]["torque_enabled"] or rows[0]["hardware_error_flags"]: + raise ValueError("Mock assignment blocked by actuator state or invalid ID") + old_id = rows[0]["servo_id"] + rows[0].update(servo_id=new_id, reported_id=new_id) + with _lock: + _mock_rows[config["port"]] = rows + return {"assigned": True, "old_id": old_id, "new_id": new_id, "actuators": rows, + "report": f"Mock ID {old_id} → {new_id} verified; physical hardware was not accessed"} + + +def _mock_release(config, expected): + rows = _mock_scan(config)["actuators"] + if len(rows) != 1 or rows[0]["servo_id"] != expected["servo_id"]: + raise ValueError("Mock actuator changed; scan again") + rows[0]["torque_enabled"] = False + with _lock: + _mock_rows[config["port"]] = rows + return {"released": True, "actuators": rows, "report": "Mock torque released; scan again before assignment"} + + +def _mock_read_position(config, servo_id): + row = next((row for row in _mock_scan(config)["actuators"] if row["servo_id"] == servo_id), None) + if row is None: + raise ValueError("Servo did not respond") + return {**row, "sampled_at": time.time(), "position_range": {"min": 0, "max": 4095}} + + +def _mock_test_context(config, state, row): + from .actuator_states import _limits + low, home, high = _limits(state) + scale = 360.0 / 4096 + joint = {"id": "actuator", "servo_id": state["servo_id"], "home_ticks": home, + "safe_min_deg": (low - home) * scale, "safe_max_deg": (high - home) * scale, + "velocity_limit": 180.0} + profile = {"id": "mock_actuator_setup", "joints": [joint], "capability_bindings": { + "joint_group": {"provider": {"package": "blacknode-robot", "component": "calibration"}}}} + row = next(row for row in _mock_scan(config)["actuators"] if row["servo_id"] == state["servo_id"]) + return {"profile": profile, "hardware_id": config["hardware_id"], "position_target_mode": True, + "calibration": {"profile_id": profile["id"], "hardware_id": config["hardware_id"], + "joints": {"actuator": joint}}, + "provider_config": {"pose": {"actuator": (row["raw_position"] - home) * scale}}, + "degrees_per_tick": scale} + + +@node(name="RobotActuatorSetupMockProvider", component="capabilities", category="Robot", hidden=True, + inputs={}, outputs={"available": Bool, "report": Text}) +def robot_actuator_setup_mock_provider(ctx): + return {"available": True, "report": "In-memory actuator setup provider for hardware-free development"} + + +robot_actuator_setup_mock_provider._bn_robot_actuator_setup_provider = { + "package": "blacknode-robot", "component": "capabilities", "scan": _mock_scan, "assign": _mock_assign, + "release": _mock_release, + "read_position": _mock_read_position, + "build_test_context": _mock_test_context, +} diff --git a/nodes/actuator_states.py b/nodes/actuator_states.py new file mode 100644 index 0000000..b66a898 --- /dev/null +++ b/nodes/actuator_states.py @@ -0,0 +1,384 @@ +"""Hardware-bound actuator calibration points, named poses, and leased motion tests.""" +from __future__ import annotations + +import atexit +import copy +import hashlib +import importlib +import math +import threading +import time + +from . import profiles + +_lock = threading.RLock() +_drafts = {} +_tests = {} +_stopped = {} +_LEASE_SECONDS = 3.0 +ACTIONS = {"state-status", "read-position", "capture-state", "save-states", "arm-test", "test-target", "test-status", "stop-test"} + + +def _motion(): + return importlib.import_module("blacknode.pkg.blacknode_motion.arm.servo_control") + + +def _identity(ctx): + from . import actuator_setup as setup + _, provider, config = setup._resolve(ctx) + servo_id = ctx.get("servo_id") + if isinstance(servo_id, bool) or not isinstance(servo_id, int) or not 0 <= servo_id <= 253: + raise ValueError("Select a discovered servo card first") + hardware = setup._NODE_REGISTRY["RobotUSBDiscovery"]({"port_filter": config["port"], "probe_open": False}) + hardware = hardware.get("hardware") or hardware + if (hardware.get("recommended") or {}).get("path") != config["port"]: + raise ValueError("Selected USB adapter is disconnected") + hardware_id = profiles._hardware_id({"hardware": hardware}) + if not hardware_id: + raise ValueError("Physical USB identity is required") + config["hardware_id"] = hardware_id + key = (hardware_id, provider["package"], provider["component"], servo_id) + return key, provider, config + + +def _path(key): + digest = hashlib.sha256("\0".join(map(str, key[:3])).encode()).hexdigest() + return profiles._profile_root() / "actuator_setups" / digest / f"servo_{key[3]}.json" + + +def _state(key): + if key not in _drafts: + path = _path(key) + draft = path.with_suffix(".draft.json") + if draft.exists(): + path = draft + data = profiles._read_json(path) if path.exists() else {} + if data and data.get("identity") != list(key): + raise ValueError("Saved setup belongs to different hardware") + _drafts[key] = data or {"schema_version": 1, "units": "ticks", "identity": list(key), "servo_id": key[3], + "points": {}, "poses": {}, "safety_margin_ticks": 0} + # Setup captures are the operator's command limits. Retire the old + # automatic inset when loading existing saved states or drafts. + _drafts[key]["safety_margin_ticks"] = 0 + return _drafts[key] + + +def _limits(state): + points = state.get("points") or {} + if not all(name in points for name in ("min", "max")): + raise ValueError("Capture both Min and Max with torque released before testing") + low, high = sorted(int(points[name]) for name in ("min", "max")) + margin = int(state.get("safety_margin_ticks", 0)) + if margin < 0 or high - low <= 2 * margin + 1: + raise ValueError("Capture distinct Min and Max positions with room for an interior test origin") + # A test origin converts raw ticks to the motion contract's joint angle. + # It is not a measured Home capture and does not rewrite captured points. + home = int(points.get("home", (low + high) // 2)) + if not low + margin < home < high - margin: + home = (low + high) // 2 + return low + margin, home, high - margin + + +def _snapshot(key, report=""): + state = copy.deepcopy(_state(key)) + try: + low, home, high = _limits(state) + limits = {"min": low, "home": home, "max": high} + except ValueError: + limits = {} + return {"ok": True, "states": state, "test_limits": limits, + "saved": bool(state.get("saved_at")) and not state.get("dirty", False), + "path": str(_path(key)), "report": report} + + +def ensure_idle(port): + with _lock: + if any(item["port"] == port for item in _tests.values()): + raise ValueError("Stop the active servo test before scanning, changing IDs or capturing released calibration") + + +def retire_hardware(hardware_id): + """Retire per-actuator setup when an ID write may change assembly identity.""" + root = profiles._profile_root() / "actuator_setups" + retired = [] + with _lock: + for path in root.glob("*/servo_*.json"): + state = profiles._read_json(path) + if (state.get("identity") or [None])[0] == hardware_id: + target = path.parent / "retired" / f"{time.time_ns()}-{path.name}" + target.parent.mkdir(parents=True, exist_ok=True) + path.rename(target) + retired.append(str(target)) + for key in list(_drafts): + if key[0] == hardware_id: + _drafts.pop(key) + return retired + + +def _read_row(provider, config, servo_id, *, strict=False, live=False): + reader = provider.get("read_position") + rows = ([reader(config, servo_id)] if live and not strict and callable(reader) + else provider["scan"](config).get("actuators") or []) + if strict and any(row.get("discovery_status") == "unreadable" for row in rows): + raise ValueError("Resolve unreadable addresses before motion testing") + row = next((row for row in rows if row.get("servo_id") == servo_id), None) + if not row or row.get("discovery_status") == "unreadable": + raise ValueError("This servo is not readable; resolve its ID and scan again") + if row.get("errors") or row.get("hardware_error_flags") or not row.get("assignment_supported"): + raise ValueError("Complete, supported, warning-free servo feedback is required") + if row.get("reported_id", servo_id) != servo_id: + raise ValueError("Servo ID readback does not match this card") + ticks = row.get("raw_position") + if isinstance(ticks, bool) or not isinstance(ticks, int): + raise ValueError("Current position is unavailable") + return row + + +def _stop(key): + item = _tests.pop(key, None) + if item is None: + return {"ok": True, "armed": False, "report": "Test stopped"} + try: + result = _motion().disarm_servo_motion(item["run_id"]) + except Exception as exc: + result = {"ok": False, "armed": False, "report": f"Torque release needs attention: {exc}"} + _stopped[key] = result + item["stop"].set() + return result + + +def _sample(item, *, cached=False): + motion = _motion() + status = motion.servo_motion_status(item["run_id"]) + if not status.get("armed"): + raise ValueError("Test is disarmed") + if cached and time.monotonic() - item.get("sample_at", 0) <= 0.25: + return dict(item["sample"]) + sample = motion.sample_servo_motion_for_robot(item["run_id"]) + if not sample or not sample.get("command_ok") or sample.get("torque_enabled") is not True: + raise ValueError((sample or {}).get("report") or "Fresh test feedback is unavailable") + row = (sample.get("servos") or {}).get("actuator") or {} + ticks = row.get("ticks") + if ticks is None and isinstance((sample.get("pose") or {}).get("actuator"), (int, float)): + ticks = round(item["limits"][1] + sample["pose"]["actuator"] / item["context"]["degrees_per_tick"]) + if isinstance(ticks, bool) or not isinstance(ticks, int): + raise ValueError("Fresh test position is unavailable") + result = {"armed": True, "raw_position": ticks, "torque_enabled": True, + "report": "Test armed. Slider moves this servo within saved limits."} + item["sample"], item["sample_at"] = result, time.monotonic() + return dict(result) + + +def _watch(key, item): + while not item["stop"].is_set(): + item["wake"].wait(0.02) + item["wake"].clear() + with _lock: + if _tests.get(key) is not item: + return + try: + if time.monotonic() - item["heartbeat"] > _LEASE_SECONDS: + raise TimeoutError("Test controls disconnected") + sample = _sample(item) + desired = item.get("desired") + if desired is not None: + direct = item.get("position_target_mode") is True + if direct and desired == item.get("last_sent_target"): + continue + scale = item["context"]["degrees_per_tick"] + current = sample["raw_position"] + # Small, bounded increments prevent a delayed UI request + # from becoming one large position jump after idle time. + elapsed = time.monotonic() - item["last_step_at"] + step = int(item["speed"] * min(0.05, elapsed) / scale) + if not direct and step < 1: + continue + target = desired if direct else max(current - step, min(current + step, desired)) + low, _, high = item["limits"] + if not low <= target <= high: + # Hold is allowed at a captured endpoint. Enter the + # inset command range only after enough time for this + # first small step; do not let driver clamping create + # a faster-than-configured jump across the margin. + target = max(low, min(high, target)) + elapsed = time.monotonic() - item["last_step_at"] + if abs(target - current) * scale > item["speed"] * elapsed: + continue + if target != current: + result = _motion().command_servo_motion(item["run_id"], { + "kind": "blacknode.joint-command-request", "schema_version": 1, + "joint_name": "actuator", "servo_id": key[3], + "position_rad": math.radians((target - item["limits"][1]) * scale), + "issued_at": time.time(), "requires_motion_authorization": True, + }) + if not result.get("ok"): + raise ValueError(result.get("report") or "Test command failed") + item["last_step_at"] = time.monotonic() + if direct: + item["last_sent_target"] = target + except Exception as exc: + stopped = _stop(key) + report = str(exc) + if not stopped.get("ok"): + report += "; " + stopped["report"] + _stopped[key] = {"ok": False, "armed": False, "report": report} + return + + +def control(ctx, action, payload): + key = None + try: + # Stop must also work after the adapter has been disconnected. + if action == "stop-test": + with _lock: + matches = [key for key, item in _tests.items() + if item["port"] == ctx.get("serial_port") and key[3] == ctx.get("servo_id")] + results = [_stop(key) for key in matches] + return next((result for result in results if not result.get("ok")), + {"ok": True, "armed": False, "report": "Test stopped; torque released" if matches + else "No active test session"}) + key, provider, config = _identity(ctx) + with _lock: + state = _state(key) + if action == "state-status": + return _snapshot(key) + if action == "read-position": + if payload.get("confirm_read_only") is not True: + raise ValueError("Confirm read-only position feedback") + ensure_idle(config["port"]) + reader = provider.get("read_position") + if not callable(reader): + raise ValueError("This provider does not support live position feedback") + row = reader(config, key[3]) + ticks = row.get("raw_position") + sampled = row.get("sampled_at", 0) + if (row.get("reported_id") != key[3] or not row.get("assignment_supported") + or row.get("discovery_status") == "unreadable" or row.get("errors") + or isinstance(ticks, bool) or not isinstance(ticks, int) + or not 0 <= time.time() - sampled <= 1.0): + raise ValueError("Fresh position from the selected actuator is unavailable") + return {"ok": True, "raw_position": ticks, "sampled_at": sampled, + "position_range": row.get("position_range"), + "torque_enabled": row.get("torque_enabled"), + "hardware_error_flags": row.get("hardware_error_flags", 0), + "report": "; ".join(row.get("hardware_errors") or [])} + if action == "capture-state": + kind = str(payload.get("point") or "pose") + if kind not in {"min", "home", "max", "pose"}: + raise ValueError("Choose Min, Home, Max or a named pose") + if kind == "pose" and key in _tests: + ticks = _sample(_tests[key])["raw_position"] + else: + ensure_idle(config["port"]) + row = _read_row(provider, config, key[3], live=True) + if row.get("torque_enabled") is not False: + raise ValueError("Support this servo and release torque before capturing calibration") + if state.get("model_number") not in (None, row["model_number"]): + raise ValueError("Actuator model changed; discard the previous setup before calibrating") + state["model_number"] = row["model_number"] + ticks = row["raw_position"] + if isinstance(ticks, bool) or not isinstance(ticks, int): + raise ValueError("Fresh position is unavailable") + if kind == "pose": + name = str(payload.get("name") or "").strip() + if not name or len(name) > 64: + raise ValueError("Give this pose a name of 1–64 characters") + state["poses"][name] = ticks + else: + state["points"][kind] = ticks + state["dirty"] = True + profiles._write_json(_path(key).with_suffix(".draft.json"), state) + return {**_snapshot(key, f"Captured {kind} at {ticks} ticks. Press Save states to keep it."), "raw_position": ticks} + if action == "save-states": + ensure_idle(config["port"]) + # Named poses can be saved before calibration is complete. + if all(name in state["points"] for name in ("min", "max")): + _limits(state) + if not state["points"] and not state["poses"]: + raise ValueError("Capture a calibration point or named pose first") + saved = {**state, "saved_at": time.time(), "dirty": False} + profiles._write_json(_path(key), saved) + _path(key).with_suffix(".draft.json").unlink(missing_ok=True) + state.update(saved) + return _snapshot(key, "Servo states saved for this USB hardware and servo ID") + if action == "arm-test": + if payload.get("confirm_test") is not True and payload.get("operator_action") != "arm-test": + raise ValueError("Confirm the calibrated actuator has a unique ID and the robot is supported") + ensure_idle(config["port"]) + low, home, high = _limits(state) + margin = int(state.get("safety_margin_ticks", 0)) + accept_draft = payload.get("operator_action") == "arm-test" and payload.get("save_calibration") is True + if (state.get("dirty") or not state.get("saved_at")) and not accept_draft: + raise ValueError("Save calibration before arming the test") + row = _read_row(provider, config, key[3]) + if row.get("model_number") != state.get("model_number"): + raise ValueError("Calibration model does not match this actuator") + if row.get("torque_enabled") is not False: + raise ValueError("Release torque before arming this test") + if not low - margin <= row["raw_position"] <= high + margin: + raise ValueError("Current position is outside the captured endpoints; update the captured range before testing") + builder = provider.get("build_test_context") + if not callable(builder): + raise ValueError("This provider does not support calibrated servo testing") + test_state = {**state, "points": {"min": low - margin, "home": home, "max": high + margin}} + motion_ctx = builder(config, test_state, row) + speeds = [float(joint.get("velocity_limit") or 0) for joint in motion_ctx["profile"]["joints"]] + speed = min([180.0 if motion_ctx.get("position_target_mode") else 60.0, *speeds]) + if not math.isfinite(speed) or speed <= 0: + raise ValueError("A bounded test speed is required") + if accept_draft and (state.get("dirty") or not state.get("saved_at")): + saved = {**state, "saved_at": time.time(), "dirty": False} + profiles._write_json(_path(key), saved) + _path(key).with_suffix(".draft.json").unlink(missing_ok=True) + state.update(saved) + run_id = "actuator-test:" + hashlib.sha256(str(key).encode()).hexdigest() + motion_ctx["robot_id"] = run_id + result = _motion().arm_servo_motion(run_id, motion_ctx) + if not result.get("ok") or not result.get("armed"): + _motion().disarm_servo_motion(run_id) + raise ValueError(result.get("report") or "Test could not arm") + item = {"run_id": run_id, "port": config["port"], "heartbeat": time.monotonic(), + "last_step_at": time.monotonic(), + "wake": threading.Event(), "speed": speed, + "position_target_mode": motion_ctx.get("position_target_mode") is True, + "stop": threading.Event(), "context": motion_ctx, "limits": (low, home, high)} + _tests[key] = item + _stopped.pop(key, None) + threading.Thread(target=_watch, args=(key, item), daemon=True, name="actuator-test").start() + return {**_snapshot(key), **_sample(item), "target_ticks": row["raw_position"]} + if action in {"test-status", "test-target"}: + item = _tests.get(key) + if item is None: + return _stopped.get(key, {"ok": True, "armed": False, "report": "Test is stopped"}) + if time.monotonic() - item["heartbeat"] > _LEASE_SECONDS: + _stop(key) + raise ValueError("Test connection expired; arm again") + item["heartbeat"] = time.monotonic() + if action == "test-target": + target = payload.get("ticks") + issued = float(payload.get("issued_at") or 0) + if isinstance(target, bool) or not isinstance(target, int) or not 0 <= time.time() - issued <= 0.75: + raise ValueError("Slider command is invalid or stale") + low, home, high = item["limits"] + if not low <= target <= high: + raise ValueError("Slider target is outside the saved safe range") + item["desired"] = target + item["wake"].set() + return {"ok": True, **_sample(item, cached=True), "target_ticks": target} + return {"ok": True, **_sample(item, cached=True)} + raise ValueError("Unsupported actuator state action") + except Exception as exc: + report = str(exc) + if key is not None and action in {"arm-test", "test-target", "test-status"}: + with _lock: + stopped = _stop(key) + if not stopped.get("ok"): + report += "; " + stopped["report"] + return {"ok": False, "armed": False, "report": report} + + +@atexit.register +def stop_runtime_services(): + with _lock: + results = [_stop(key) for key in list(_tests)] + return {"ok": all(result.get("ok") for result in results), "stopped": {"managed_runs": len(results)}} diff --git a/nodes/calibration_control.py b/nodes/calibration_control.py index 2be9744..1dd49ec 100644 --- a/nodes/calibration_control.py +++ b/nodes/calibration_control.py @@ -374,6 +374,12 @@ def command( self.pose[name] = float(value) return self.sample() + def command_position_target(self, positions_deg: Mapping[str, float], *, + max_velocity_deg_s: float, deadline: float) -> dict[str, Any]: + if not 0 < max_velocity_deg_s <= 180: + raise ValueError("Mock position tracking speed is invalid") + return self.command(positions_deg, deadline=deadline) + def close(self) -> None: self.torque_enabled = False @@ -411,6 +417,7 @@ def robot_calibration_mock_provider(ctx: dict) -> dict: } robot_calibration_mock_provider._bn_robot_joint_motion_provider = { + "supports_position_targets": True, "package": "blacknode-robot", "component": "calibration", "capability": "joint_group", diff --git a/pyproject.toml b/pyproject.toml index 66d2087..3bc44b1 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "blacknode-robot" -version = "0.5.7" +version = "0.5.8" description = "Robot contracts, connected-device lifecycle, and normalized telemetry for Blacknode." requires-python = ">=3.11" dependencies = ["pyserial>=3.5", "feetech-servo-sdk>=1.0", "roslibpy>=1.5"] diff --git a/templates/actuator-setup.json b/templates/actuator-setup.json new file mode 100644 index 0000000..0dbc2c9 --- /dev/null +++ b/templates/actuator-setup.json @@ -0,0 +1,26 @@ +{ + "kind": "blacknode.workflow", + "schema_version": 1, + "name": "Actuator Setup", + "entrypoint": {"node_id": "setup", "port": "report"}, + "metadata": { + "template": true, + "description": "Pick a USB port and press Scan. Each servo gets its own ID controls, Min/Home/Max calibration capture, named poses, Save states and an explicitly armed test slider within saved limits. Running the workflow is inert.", + "color": "#14b8a6", + "required_packages": ["blacknode-robot", "blacknode-drivers", "blacknode-motion"], + "required_components": ["blacknode-robot/core", "blacknode-robot/capabilities", "blacknode-robot/calibration", "blacknode-robot/devices", "blacknode-drivers/feetech", "blacknode-motion/arm"] + }, + "node_meta": { + "setup": { + "id": "setup", "type": "ActuatorSetup", "pos": [80, 80], + "params": {"profile_id": "", "serial_port": "", "baudrate": 1000000}, + "inputs": ["profile_id", "serial_port", "baudrate"], + "outputs": ["ok", "assigned", "actuators", "profile", "bus", "report"], + "input_types": {"profile_id": "Text", "serial_port": "Text", "baudrate": "Int"}, + "output_types": {"ok": "Bool", "assigned": "Bool", "actuators": "List", "profile": "Dict", "bus": "Dict", "report": "Text"}, + "input_defaults": {"profile_id": "", "serial_port": "", "baudrate": 1000000}, + "primary_inputs": [], "primary_outputs": ["bus", "report"] + } + }, + "edges": [] +} diff --git a/tests/test_actuator_states.py b/tests/test_actuator_states.py new file mode 100644 index 0000000..39e3c62 --- /dev/null +++ b/tests/test_actuator_states.py @@ -0,0 +1,320 @@ +import time + +import blacknode # noqa: F401 +import pytest +from blacknode.pkg.blacknode_robot import actuator_setup as setup +from blacknode.pkg.blacknode_robot import actuator_states as states + + +@pytest.fixture +def rig(monkeypatch, tmp_path): + states.stop_runtime_services() + states._drafts.clear() + states._stopped.clear() + setup._mock_rows.clear() + setup._mock_scan({"port": "fake"}) + key = ("physical-123", "blacknode-robot", "capabilities", 1) + provider = setup.robot_actuator_setup_mock_provider._bn_robot_actuator_setup_provider + config = {"port": "fake", "baudrate": 1000000, "hardware_id": key[0]} + monkeypatch.setattr(states, "_identity", lambda _: (key, provider, config)) + monkeypatch.setattr(states.profiles, "_profile_root", lambda: tmp_path) + ctx = {"serial_port": "fake", "servo_id": 1} + yield ctx, key, setup._mock_rows["fake"][0] + states.stop_runtime_services() + states._drafts.clear() + + +def call(rig, action, **payload): + return states.control(rig[0], action, payload) + + +def calibrated(rig): + for point, ticks in (("min", 1000), ("home", 2000), ("max", 3000)): + rig[2]["raw_position"] = ticks + assert call(rig, "capture-state", point=point)["ok"] + rig[2]["raw_position"] = 2000 + assert call(rig, "save-states")["saved"] + + +def test_capture_and_save_reload_hardware_bound_calibration_and_named_pose(rig): + calibrated(rig) + result = call(rig, "capture-state", point="pose", name="Ready") + assert result["states"]["poses"] == {"Ready": 2000} and not result["saved"] + result = call(rig, "save-states") + assert result["test_limits"] == {"min": 1000, "home": 2000, "max": 3000} + states._drafts.clear() + loaded = call(rig, "state-status") + assert loaded["saved"] and loaded["states"]["poses"] == {"Ready": 2000} + assert loaded["states"]["identity"] == list(rig[1]) + assert rig[2]["torque_enabled"] is False + + +@pytest.mark.parametrize("draft", [False, True]) +def test_legacy_automatic_margin_loads_exact_endpoints_and_saves_them(rig, draft): + calibrated(rig) + path = states._path(rig[1]) + legacy = states.profiles._read_json(path) + legacy["safety_margin_ticks"] = 20 + if draft: + path = path.with_suffix(".draft.json") + legacy["dirty"] = True + states.profiles._write_json(path, legacy) + states._drafts.clear() + loaded = call(rig, "state-status") + assert loaded["test_limits"] == {"min": 1000, "home": 2000, "max": 3000} + assert loaded["states"]["points"] == legacy["points"] + assert loaded["states"]["safety_margin_ticks"] == 0 + assert call(rig, "save-states")["saved"] + assert states.profiles._read_json(states._path(rig[1]))["safety_margin_ticks"] == 0 + + +def test_partial_capture_survives_reload_as_draft_until_explicit_save(rig): + result = call(rig, "capture-state", point="home") + states._drafts.clear() + loaded = call(rig, "state-status") + assert loaded["states"]["points"] == result["states"]["points"] + assert loaded["states"]["dirty"] and not loaded["saved"] + assert not call(rig, "arm-test", operator_action="arm-test")["ok"] + assert call(rig, "save-states")["saved"] + assert not states._path(rig[1]).with_suffix(".draft.json").exists() + + +def test_live_hand_position_before_calibration_never_arms_or_captures(rig): + for ticks in (500, 3500): + rig[2]["raw_position"] = ticks + result = call(rig, "read-position", confirm_read_only=True) + assert result["ok"] and result["raw_position"] == ticks + assert result["torque_enabled"] is False + assert result["position_range"] == {"min": 0, "max": 4095} + assert 0 <= time.time() - result["sampled_at"] < 1 + assert not states._tests + assert not call(rig, "state-status")["states"]["points"] + assert not call(rig, "read-position")["ok"] + + +def test_live_position_does_not_open_another_connection_during_motion(rig): + calibrated(rig) + assert call(rig, "arm-test", operator_action="arm-test")["armed"] + assert not call(rig, "read-position", confirm_read_only=True)["ok"] + assert states._tests + + +def test_partial_points_can_be_saved_but_cannot_arm(rig): + assert call(rig, "capture-state", point="home")["ok"] + assert call(rig, "save-states")["saved"] + assert not call(rig, "arm-test", confirm_test=True)["ok"] + + +@pytest.mark.parametrize("change", ["torque", "warning", "unreadable", "id_mismatch"]) +def test_invalid_feedback_does_not_capture_calibration(rig, change): + if change == "torque": rig[2]["torque_enabled"] = True + if change == "warning": rig[2]["hardware_error_flags"] = 32 + if change == "unreadable": rig[2]["discovery_status"] = "unreadable" + if change == "id_mismatch": rig[2]["reported_id"] = 2 + assert not call(rig, "capture-state", point="home")["ok"] + assert not call(rig, "state-status")["states"]["points"] + + +def test_invalid_range_does_not_save_or_arm(rig): + for point in ("min", "home", "max"): + assert call(rig, "capture-state", point=point)["ok"] + assert not call(rig, "save-states")["ok"] + assert not call(rig, "arm-test", confirm_test=True)["ok"] + + +def test_arm_requires_explicit_confirmation_and_saved_matching_model(rig): + calibrated(rig) + assert not call(rig, "arm-test")["ok"] + rig[2]["model_number"] = 123 + assert not call(rig, "arm-test", confirm_test=True)["ok"] + + +def test_arm_button_is_explicit_authorization_without_an_extra_checkbox(rig): + calibrated(rig) + result = call(rig, "arm-test", operator_action="arm-test") + assert result["ok"] and result["armed"] + assert call(rig, "stop-test")["ok"] + + +@pytest.mark.parametrize("home", [None, 4038, 3300]) +def test_arm_accepts_reversed_endpoints_and_derives_test_origin_without_rewriting_captures(rig, home): + points = {"min": 4038, "max": 2607} + if home is not None: + points["home"] = home + for point, ticks in points.items(): + rig[2]["raw_position"] = ticks + assert call(rig, "capture-state", point=point)["ok"] + rig[2]["raw_position"] = 3000 + result = call(rig, "arm-test", operator_action="arm-test", save_calibration=True) + assert result["ok"] and result["armed"] and result["saved"], result + assert result["test_limits"] == {"min": 2607, "home": 3300 if home == 3300 else 3322, "max": 4038} + assert result["states"]["points"] == points + assert result["target_ticks"] == result["raw_position"] == 3000 + assert states.profiles._read_json(states._path(rig[1]))["points"] == points + + +@pytest.mark.parametrize("start,target", [(1000, 3000), (3000, 1000)]) +def test_arm_holds_endpoint_then_sends_direct_target_once(rig, monkeypatch, start, target): + calibrated(rig) + rig[2]["raw_position"] = start + commands = [] + original = states._motion().command_servo_motion + def command(run_id, payload): + commands.append((time.monotonic(), payload)) + return original(run_id, payload) + monkeypatch.setattr(states._motion(), "command_servo_motion", command) + result = call(rig, "arm-test", operator_action="arm-test", save_calibration=True) + assert result["ok"] and result["armed"], result + assert result["raw_position"] == result["target_ticks"] == start + time.sleep(0.12) + assert not commands + result = call(rig, "test-target", ticks=target, issued_at=time.time()) + assert result["ok"] + deadline = time.monotonic() + 2 + while result["raw_position"] != target and time.monotonic() < deadline: + time.sleep(0.02) + result = call(rig, "test-status") + assert result["raw_position"] == target and result["armed"], result + assert len(commands) == 1 + + +def test_direct_slider_target_does_not_walk_through_intermediate_positions(rig, monkeypatch): + calibrated(rig) + sent = [] + original = states._motion().command_servo_motion + def command(run_id, payload): + sent.append(payload["position_rad"]) + return original(run_id, payload) + monkeypatch.setattr(states._motion(), "command_servo_motion", command) + assert call(rig, "arm-test", operator_action="arm-test")["armed"] + assert call(rig, "test-target", ticks=2900, issued_at=time.time())["ok"] + deadline = time.monotonic() + 1 + while time.monotonic() < deadline: + result = call(rig, "test-status") + if result.get("raw_position") == 2900: break + time.sleep(0.01) + assert result["raw_position"] == 2900 and result["armed"] + assert len(sent) == 1 + + +def test_slider_ack_and_status_reuse_fresh_feedback_without_extra_bus_reads(rig, monkeypatch): + calibrated(rig) + assert call(rig, "arm-test", operator_action="arm-test")["armed"] + # Hold the worker lock so only request-path I/O is measured here. + with states._lock: + def unexpected(*args): raise AssertionError("Redundant serial feedback read") + monkeypatch.setattr(states._motion(), "sample_servo_motion_for_robot", unexpected) + assert call(rig, "test-target", ticks=2010, issued_at=time.time())["ok"] + assert call(rig, "test-target", ticks=2020, issued_at=time.time())["ok"] + assert call(rig, "test-status")["ok"] + item = states._tests[rig[1]] + assert item["desired"] == 2020 and item["wake"].is_set() + # Old feedback must trigger a real read and fail closed, never be reused. + item["sample_at"] -= 1 + assert not call(rig, "test-status")["ok"] + assert not states._tests + + +def test_arm_accept_draft_keeps_captured_limits_and_does_not_save_on_failed_preflight(rig): + for point, ticks in (("min", 1000), ("max", 3000)): + rig[2]["raw_position"] = ticks + assert call(rig, "capture-state", point=point)["ok"] + rig[2]["raw_position"] = 3001 + result = call(rig, "arm-test", operator_action="arm-test", save_calibration=True) + assert not result["ok"] and not result["armed"] and not states._tests + assert "outside the captured endpoints" in result["report"] + assert not call(rig, "state-status")["saved"] + + +def test_arm_accept_draft_never_arms_if_save_fails(rig, monkeypatch): + for point, ticks in (("min", 1000), ("max", 3000)): + rig[2]["raw_position"] = ticks + assert call(rig, "capture-state", point=point)["ok"] + rig[2]["raw_position"] = 2000 + def fail(*args): raise OSError("Disk full") + monkeypatch.setattr(states.profiles, "_write_json", fail) + result = call(rig, "arm-test", operator_action="arm-test", save_calibration=True) + assert not result["ok"] and not result["armed"] and not states._tests + assert not call(rig, "state-status")["saved"] + + +def test_arm_button_cannot_bypass_missing_calibration(rig): + result = call(rig, "arm-test", operator_action="arm-test", save_calibration=True) + assert not result["ok"] and not result["armed"] and not states._tests + + +def test_mock_motion_moves_only_selected_joint_and_captures_measured_pose(rig): + pytest.importorskip("blacknode.pkg.blacknode_motion.arm.servo_control") + calibrated(rig) + result = call(rig, "arm-test", confirm_test=True) + assert result["ok"] and result["armed"], result + assert result["target_ticks"] == 2000 + result = call(rig, "test-target", ticks=2001, issued_at=time.time()) + assert result["ok"] and result["armed"] and isinstance(result["raw_position"], int), result + deadline = time.monotonic() + 2 + while result["raw_position"] != 2001 and time.monotonic() < deadline: + time.sleep(0.05) + result = call(rig, "test-status") + assert result["raw_position"] == 2001 + captured = call(rig, "capture-state", point="pose", name="Tested") + assert captured["ok"] and captured["states"]["poses"]["Tested"] == result["raw_position"] + assert not call(rig, "capture-state", point="min")["ok"] + assert not call(rig, "save-states")["ok"] + assert call(rig, "stop-test")["ok"] + assert not states._tests + assert call(rig, "save-states")["saved"] + + +@pytest.mark.parametrize("target,issued_delta", [(999, 0), (3001, 0), (True, 0), (2000, -5)]) +def test_bad_slider_command_disarms_and_closes_session(rig, target, issued_delta): + calibrated(rig) + assert call(rig, "arm-test", confirm_test=True)["armed"] + result = call(rig, "test-target", ticks=target, issued_at=time.time() + issued_delta) + assert not result["ok"] and not result["armed"] and not states._tests + + +def test_deadman_releases_when_controls_disconnect(rig, monkeypatch): + calibrated(rig) + monkeypatch.setattr(states, "_LEASE_SECONDS", 0.02) + assert call(rig, "arm-test", confirm_test=True)["armed"] + item = states._tests[rig[1]] + assert item["stop"].wait(2), "watchdog must stop the test" + assert not states._tests + assert not states._motion().servo_motion_status(item["run_id"])["armed"] + + +def test_id_change_retires_saved_points_and_named_poses(rig): + calibrated(rig) + path = states._path(rig[1]) + assert path.exists() + assert states.retire_hardware(rig[1][0]) + assert not path.exists() and not states._drafts + assert not call(rig, "state-status")["states"]["points"] + + +def test_failed_disk_save_keeps_draft_unsaved(rig, monkeypatch): + call(rig, "capture-state", point="home") + def fail(*args): raise OSError("Disk full") + monkeypatch.setattr(states.profiles, "_write_json", fail) + assert not call(rig, "save-states")["ok"] + assert not call(rig, "state-status")["saved"] + + +def test_unreadable_other_address_does_not_hide_healthy_calibrated_servo(rig): + calibrated(rig) + setup._mock_rows["fake"].append({"servo_id": 99, "discovery_status": "unreadable"}) + assert call(rig, "arm-test", confirm_test=True)["armed"] + + +def test_watchdog_preserves_torque_release_failure_report(rig, monkeypatch): + calibrated(rig) + assert call(rig, "arm-test", confirm_test=True)["armed"] + original = states._motion().disarm_servo_motion + def release_failed(run_id): + original(run_id) + return {"ok": False, "armed": False, "report": "Torque release was not verified"} + monkeypatch.setattr(states._motion(), "disarm_servo_motion", release_failed) + states._tests[rig[1]]["heartbeat"] -= 10 + item = states._tests[rig[1]] + assert item["stop"].wait(2) + assert "not verified" in call(rig, "test-status")["report"] diff --git a/tests/test_robot_actuator_setup.py b/tests/test_robot_actuator_setup.py new file mode 100644 index 0000000..5e63dbc --- /dev/null +++ b/tests/test_robot_actuator_setup.py @@ -0,0 +1,187 @@ +import copy +import json + +import pytest +import blacknode # noqa: F401 +from blacknode.node import _NODE_REGISTRY +from blacknode.pkg.blacknode_robot import actuator_setup as setup + + +@pytest.fixture +def context(monkeypatch, tmp_path): + profile = setup.profiles.builtin_profile("so_arm101") + profile["capability_bindings"] = {"joint_group": {"provider": { + "package": "blacknode-robot", "component": "capabilities"}}} + monkeypatch.setattr(setup.profiles, "load_profile", lambda name: (copy.deepcopy(profile), None)) + monkeypatch.setattr(setup.profiles, "_profile_roots", lambda: [tmp_path]) + monkeypatch.setattr(setup.profiles, "list_profiles", lambda: [{"id": "so_arm101"}]) + monkeypatch.setitem(_NODE_REGISTRY, "RobotUSBDiscovery", lambda ctx: { + "recommended": {"path": "fake", "serial": "physical-123"}}) + setup._tickets.clear() + setup._mock_rows.clear() + return {"profile_id": "so_arm101", "serial_port": "fake", "baudrate": 1000000} + + +def discover(ctx): + return setup.control_actuator_setup(ctx, "scan", {"confirm_read_only": True}) + + +def confirmation(result): + return {"scan_token": result["scan_token"], "joint_id": "gripper", "confirm_read_only": True, + "confirm_isolated": True, "confirm_recalibrate": True} + + +def test_saved_workflow_cook_is_inert_even_with_injected_action(context, monkeypatch): + monkeypatch.setattr(setup, "_mock_scan", lambda _: pytest.fail("must not scan")) + result = setup.actuator_setup({**context, "action": "assign", "confirm_isolated": True}) + assert result["ok"] and not result["assigned"] and not setup._tickets + + +def test_mock_provider_normalized_contract_and_single_use_authorization(context, tmp_path): + path = tmp_path / "so_arm101/calibrations/physical_123.json" + path.parent.mkdir(parents=True) + path.write_text(json.dumps({"hardware_id": "physical-123", "joints": {}})) + scan = discover(context) + assert scan["ok"] and scan["actuators"][0]["servo_id"] == 1 + payload = confirmation(scan) + result = setup.control_actuator_setup(context, "assign", payload) + assert result["ok"] and result["assigned"] and result["new_id"] == 6 + assert result["actuators"][0]["torque_enabled"] is False + assert not path.exists() and len(result["retired_calibrations"]) == 1 + assert not setup.control_actuator_setup(context, "assign", payload)["ok"] + assert discover(context)["actuators"][0]["servo_id"] == 6 + + +@pytest.mark.parametrize("field", ["confirm_isolated", "confirm_recalibrate", "confirm_read_only"]) +def test_missing_confirmation_never_assigns(context, field): + payload = confirmation(discover(context)) + payload[field] = False + result = setup.control_actuator_setup(context, "assign", payload) + assert not result["ok"] and setup._mock_scan({"port": "fake"})["actuators"][0]["servo_id"] == 1 + + +def test_expired_ticket_blocks_assignment(context, monkeypatch): + payload = confirmation(discover(context)) + now = setup.time.monotonic() + monkeypatch.setattr(setup.time, "monotonic", lambda: now + 121) + result = setup.control_actuator_setup(context, "assign", payload) + assert not result["ok"] and "expired" in result["report"] + + +def test_changed_port_baud_or_hardware_invalidates_scan(context): + payload = confirmation(discover(context)) + result = setup.control_actuator_setup({**context, "baudrate": 115200}, "assign", payload) + assert not result["ok"] and "selection changed" in result["report"] + + +def test_provider_absence_is_structured_and_cook_opens_no_hardware(context, monkeypatch): + monkeypatch.delitem(_NODE_REGISTRY, "RobotActuatorSetupMockProvider") + result = setup.actuator_setup(context) + assert not result["ok"] and "provider unavailable" in result["report"] + + +def test_scan_invalidates_older_confirmation_for_same_hardware(context): + first = discover(context) + second = discover(context) + assert first["scan_token"] != second["scan_token"] + assert not setup.control_actuator_setup(context, "assign", confirmation(first))["ok"] + + +def test_multiple_actuators_and_invalid_joint_block_assignment(context): + setup._mock_rows["fake"] = [{"servo_id": 1}, {"servo_id": 2}] + result = setup.control_actuator_setup(context, "assign", confirmation(discover(context))) + assert not result["ok"] and "exactly one" in result["report"] + setup._mock_rows.clear() + payload = {**confirmation(discover(context)), "joint_id": "unknown"} + assert not setup.control_actuator_setup(context, "assign", payload)["ok"] + + +def test_profile_defaults_resolve_feetech_without_sdk_access(monkeypatch): + monkeypatch.setattr(setup.profiles, "load_profile", lambda name: (setup.profiles.builtin_profile(name), None)) + if "FeetechActuatorSetupProvider" not in _NODE_REGISTRY: + pytest.skip("driver package not installed") + assert setup.actuator_setup({"profile_id": "so_arm101", "serial_port": "fake"})["ok"] + + +def test_release_requires_isolation_and_consumes_scan(context): + rows = setup._mock_scan({"port": "fake"})["actuators"] + rows[0]["torque_enabled"] = True + setup._mock_rows["fake"] = rows + payload = confirmation(discover(context)) + assert not setup.control_actuator_setup(context, "release", {**payload, "confirm_isolated": False})["ok"] + result = setup.control_actuator_setup(context, "release", {**payload, "confirm_recalibrate": False}) + assert result["ok"] and result["released"] and not result["assigned"] + assert not setup.control_actuator_setup(context, "assign", payload)["ok"] + + +def test_usb_only_scan_and_numeric_id_need_no_profile(context, monkeypatch): + provider = dict(setup.robot_actuator_setup_mock_provider._bn_robot_actuator_setup_provider) + provider["match_hardware"] = lambda hardware: 100 if hardware["port"] == "fake" else 0 + monkeypatch.setattr(setup.robot_actuator_setup_mock_provider, "_bn_robot_actuator_setup_provider", provider) + monkeypatch.setattr(setup.profiles, "load_profile", lambda _: pytest.fail("USB discovery must not need a profile")) + monkeypatch.setattr(setup.profiles, "list_profiles", lambda: []) + ctx = {"serial_port": "fake", "servo_id": 1} + scan = discover(ctx) + assert scan["ok"] and scan["profile"] == {} and scan["bus"]["port"] == "fake" + result = setup.control_actuator_setup(ctx, "assign", {**confirmation(scan), "new_id": 7}) + assert result["ok"] and result["new_id"] == 7 + + +def test_servo_card_cannot_program_another_isolated_servo(context): + scan = discover(context) + result = setup.control_actuator_setup({**context, "servo_id": 2}, "assign", { + **confirmation(scan), "new_id": 7}) + assert not result["ok"] and "does not match" in result["report"] + assert discover(context)["actuators"][0]["servo_id"] == 1 + + +@pytest.mark.parametrize("new_id", [0, 254, True, "6", 1.5]) +def test_direct_id_rejects_invalid_values(context, new_id): + result = setup.control_actuator_setup(context, "assign", { + **confirmation(discover(context)), "new_id": new_id}) + assert not result["ok"] and "1 to 253" in result["report"] + + +def test_scan_keeps_unresolved_addresses_separate_from_confirmed_ids(context): + setup._mock_rows["fake"] = [{"servo_id": 1, "discovery_status": "unreadable"}, + {"servo_id": 2, "model": "Mock"}] + result = discover(context) + assert result["ok"] and len(result["actuators"]) == 2 + assert "1 responding IDs" in result["report"] and "Unreadable replies at IDs 1" in result["report"] + assert 1 in result["missing_ids"] and 2 not in result["missing_ids"] + + +@pytest.mark.parametrize("action", ["assign", "release"]) +def test_unresolved_address_cannot_authorize_writes(context, action): + setup._mock_rows["fake"] = [{"servo_id": 1, "discovery_status": "unreadable"}] + result = setup.control_actuator_setup(context, action, confirmation(discover(context))) + assert not result["ok"] and "address is unresolved" in result["report"] + + +def test_set_id_button_scans_and_assigns_without_checkboxes_or_stale_tokens(context): + result = setup.control_actuator_setup({**context, "servo_id": 1}, "assign", { + "operator_action": "assign", "new_id": 6, "confirm_read_only": True}) + assert result["ok"] and result["assigned"] and result["new_id"] == 6 + assert not setup._tickets + + +@pytest.mark.parametrize("problem", ["multiple", "unreadable", "different_id", "torque", "warning"]) +def test_set_id_button_checks_actual_bus_before_writing(context, problem): + setup._mock_scan({"port": "fake"}) + rows = setup._mock_rows["fake"] + if problem == "multiple": rows.append({"servo_id": 2}) + if problem == "unreadable": rows[0]["discovery_status"] = "unreadable" + if problem == "different_id": rows[0]["servo_id"] = 2 + if problem == "torque": rows[0]["torque_enabled"] = True + if problem == "warning": rows[0]["hardware_error_flags"] = 32 + result = setup.control_actuator_setup({**context, "servo_id": 1}, "assign", { + "operator_action": "assign", "new_id": 6, "confirm_read_only": True}) + assert not result["ok"] and not result["assigned"] + assert setup._mock_rows["fake"][0]["servo_id"] != 6 + + +def test_setting_current_id_keeps_existing_calibration(context, monkeypatch): + monkeypatch.setattr(setup, "_retire_calibrations", lambda _: pytest.fail("unchanged ID must preserve calibration")) + result = setup.control_actuator_setup({**context, "servo_id": 1}, "assign", { + "operator_action": "assign", "new_id": 1, "confirm_read_only": True}) + assert result["ok"] and not result["retired_calibrations"] diff --git a/tests/test_robot_template_device_selection.py b/tests/test_robot_template_device_selection.py index 8f8b6cd..6ef39cc 100644 --- a/tests/test_robot_template_device_selection.py +++ b/tests/test_robot_template_device_selection.py @@ -7,6 +7,7 @@ _TEMPLATE_DIR = Path(__file__).resolve().parents[1] / "templates" _EXPECTED_ROBOTS = { + "actuator-setup.json": {}, "complete-robot-bringup.json": {"robot": 0}, "compute-device-inspection.json": {}, "editable-so-arm101-profile.json": {},