#!/usr/bin/env python3 """ 4a_base_slider.py ----------------- Estimate the Base linear-slider position (variable 'x', in mm) from observed world-space marker positions produced by step 3. Mathematical principle ---------------------- The Base slides along the world x-axis. Every joint between Base and the observable arm markers rotates around the x-axis (axis ≈ [-1,0,0] for Arm1, Ellbow, Arm2). Rotation around the x-axis NEVER changes the x-coordinate of any point. Therefore, for every marker on Arm1, Ellbow, or Arm2: world_x = x_slider + link_frame_x_at_zero + local_x where link_frame_x_at_zero is the x of the link-frame origin in world when all joint variables are zero (constant, readable from robot.json). Rearranging: x_slider = world_x - link_frame_x_at_zero - local_x This holds for ANY rotation angle y, z, a of the arm links. This gives one independent estimate per observed marker. We report their mean (and statistics) as the final estimate. Usage ----- python 4a_base_slider.py --robot robot.json --aruco aruco_positions_optimized.json python 4a_base_slider.py --robot robot.json --aruco aruco_positions_optimized.json \\ --links Arm1 Ellbow Arm2 --output 4a_result.json """ from __future__ import annotations import argparse import json import math from pathlib import Path from typing import Dict, List, Optional import numpy as np from robot_fk import RobotFK # ────────────────────────────────────────────────────────────── # I/O helpers # ────────────────────────────────────────────────────────────── def _load_json(path: Path) -> dict: with path.open("r", encoding="utf-8") as f: return json.load(f) def _load_observed_mm(aruco_json: dict) -> Dict[int, np.ndarray]: """marker_id → world position in mm, from aruco_positions_optimized.json.""" result: Dict[int, np.ndarray] = {} for m in aruco_json.get("markers", []): mid = int(m.get("marker_id", m.get("id", -1))) if mid < 0: continue if "position_mm" in m: result[mid] = np.array(m["position_mm"], dtype=float) elif "position_m" in m: result[mid] = np.array(m["position_m"], dtype=float) * 1000.0 return result # ────────────────────────────────────────────────────────────── # Core estimator # ────────────────────────────────────────────────────────────── def estimate_base_slider( fk: RobotFK, observed_mm: Dict[int, np.ndarray], use_links: Optional[List[str]] = None, verbose: bool = True, ) -> dict: """ Parameters ---------- fk : RobotFK instance observed_mm : dict marker_id -> world position [mm] use_links : links whose markers are used (default: Arm1, Ellbow, Arm2) All must be connected to Base via only x-axis rotations. verbose : print breakdown Returns ------- dict with keys: status, x_mean_mm, x_median_mm, x_std_mm, num_markers, per_marker """ if use_links is None: use_links = ["Arm1", "Ellbow", "Arm2"] # Pre-compute link_frame_x at zero-state for each target link link_x0: Dict[str, float] = {} for lname in use_links: if lname in fk.links: link_x0[lname] = fk.link_x_at_zero_state(lname) estimates: List[float] = [] per_marker: List[dict] = [] for lname in use_links: if lname not in fk.links or lname not in link_x0: continue x0 = link_x0[lname] for m in fk.links[lname].get("markers", []): mid = int(m.get("id", -1)) if mid < 0 or "position" not in m: continue if mid not in observed_mm: continue # marker not seen in this pose local_x = float(m["position"][0]) world_x = float(observed_mm[mid][0]) x_est = world_x - x0 - local_x estimates.append(x_est) per_marker.append({ "marker_id": mid, "link": lname, "local_x_mm": local_x, "link_x0_mm": x0, "world_x_mm": world_x, "x_estimate_mm": x_est, }) if not estimates: msg = (f"No observed markers found on links {use_links}. " "Check that aruco_positions_optimized.json contains " "markers from those links.") if verbose: print(f"[4a FAILED] {msg}") return {"status": "failed", "reason": msg, "use_links": use_links} arr = np.array(estimates) x_mean = float(np.mean(arr)) x_median = float(np.median(arr)) x_std = float(np.std(arr, ddof=0)) if verbose: print("\n── 4a: Base slider (x) ──────────────────────────────────") print(f" Links used: {use_links}") print(f" Markers used: {len(estimates)}") print(f" x = {x_mean:+.2f} mm (median {x_median:+.2f}, σ {x_std:.2f})") if x_std > 5.0: print(f" [WARN] high spread ({x_std:.1f} mm) – step-3 errors or wrong link list") print(f"\n Per-marker:") print(f" {'ID':>5} {'Link':8} {'local_x':>8} {'world_x':>8} {'x_est':>8}") print(f" " + "-"*46) for pm in per_marker: print(f" {pm['marker_id']:>5} {pm['link']:8} " f"{pm['local_x_mm']:>8.2f} {pm['world_x_mm']:>8.2f} {pm['x_estimate_mm']:>8.2f}") return { "status": "ok", "joint": "x", "description": "Base linear slider", "x_mean_mm": x_mean, "x_median_mm": x_median, "x_std_mm": x_std, "num_markers": len(estimates), "use_links": use_links, "per_marker": per_marker, } # ────────────────────────────────────────────────────────────── # CLI # ────────────────────────────────────────────────────────────── def main() -> int: p = argparse.ArgumentParser(description="4a: estimate Base slider (x)") p.add_argument("--robot", type=Path, default=Path("robot.json")) p.add_argument("--aruco", type=Path, default=Path("aruco_positions_optimized.json")) p.add_argument("--links", nargs="+", default=["Arm1", "Ellbow", "Arm2"], dest="links", help="Links whose markers to use (must be x-axis-rotation chain)") p.add_argument("--output", type=Path, default=None, help="Optional JSON output path (e.g. 4a_result.json)") args = p.parse_args() fk = RobotFK.from_file(args.robot) aruco_json = _load_json(args.aruco) observed_mm = _load_observed_mm(aruco_json) result = estimate_base_slider(fk, observed_mm, use_links=args.links) if args.output and result["status"] == "ok": args.output.parent.mkdir(parents=True, exist_ok=True) with args.output.open("w", encoding="utf-8") as f: json.dump(result, f, indent=2) print(f"\n Saved → {args.output}") return 0 if result["status"] == "ok" else 1 if __name__ == "__main__": raise SystemExit(main())