208 lines
7.7 KiB
Python
208 lines
7.7 KiB
Python
#!/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())
|