ChK/appRobotHoming
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

MultiPose

Browse Source
This commit is contained in:
chk
2026-06-16 17:04:11 +02:00
parent 08d1c21d1e
commit 5f6d28673a
5 changed files with 487 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

258
scripts/5_pose_estimation.py
View File

@@ -24,6 +24,26 @@ Observation input:
marker_observation = "corner_pose" -> aruco_marker_poses.json (pos + measured normal)
marker_observation = "center_point" -> aruco_positions_*.json (pos only)
Homing integration (appRobotHoming, see doc/Homing_5_Pose.md):
--from-state <json> seed/init state (flat {var: value}, or the
{"accumulated_state": {...}} shape written by
4b_revolute_angle.py) used as x0 for
global_ba/hybrid instead of the internal
estimate_sequential_fk() cold start. Missing
variables default to 0 and are estimated/flagged
normally. Without --from-state, behaviour is
unchanged (internal cold start, as before).
--calibrate-origin <Link> special mode: instead of estimating the full
pose, fit <Link>'s own joint value TOGETHER WITH
its jointToParent.origin Y/Z from that link's own
markers (complements the geometric multi-pose
method in doc/Kalibrierung.md Schritt [4]).
Writes a *_origin_calibration.json report; never
modifies robot.json.
Unobservable joints (confidence "none") are written as value=null in the
output JSON — never a fabricated 0 (see movements.<var>.observable).
Both the engine (estimate_pose) and a CLI (main) live here.
"""
from __future__ import annotations
@@ -109,6 +129,22 @@ def load_observations(path: str, use_normals: bool, min_cams: int = 2) -> Dict[i
return out
def load_seed_state(path: str) -> Dict[str, float]:
"""
Load a partial/full joint state to use as an optimisation seed (--from-state).
Accepts either a flat {variable: value} dict, or the
{"accumulated_state": {...}, ...} wrapper written by 4b_revolute_angle.py —
same unwrap rule as server/homingOrchestrator.js
(`stateData.accumulated_state ?? stateData`), so 4b's output files can be
passed in directly. Unknown keys are ignored; missing STATE_KEYS are simply
absent from the returned dict (caller defaults them, e.g. to 0.0).
"""
data = json.load(open(path, "r", encoding="utf-8"))
raw = data.get("accumulated_state", data) if isinstance(data, dict) else {}
return {k: float(v) for k, v in raw.items() if k in STATE_KEYS and v is not None}
# ==================================================================
# Kinematic chain analysis
# ==================================================================
@@ -270,9 +306,23 @@ def _multistart_values(vtype: str) -> List[float]:
def estimate_sequential_fk(fk: RobotFK, obs: Dict[int, Dict[str, Any]], chain: Dict[str, Any],
cfg: Dict[str, Any]) -> Dict[str, float]:
"""Estimate block by block along the chain, freezing already-solved variables."""
cfg: Dict[str, Any], seed: Optional[Dict[str, float]] = None
) -> Dict[str, float]:
"""
Estimate block by block along the chain, freezing already-solved variables.
seed: optional partial/full state (e.g. from 4b_revolute_angle.py) to trust
as a starting point. A block is SKIPPED entirely (seed used as-is, no
re-fit) only if ALL of its variables are present in seed. Blocks with any
missing variable are still fit normally — including their own multi-start
— but using the seeded values of EARLIER blocks as fixed context instead
of 0. This keeps the local-minimum protection for whatever the seed does
NOT cover (see doc/Homing_5_Pose.md "Wichtige Einschraenkung"), while not
re-perturbing values the caller already trusts.
"""
state = {k: 0.0 for k in STATE_KEYS}
if seed:
state.update({k: v for k, v in seed.items() if k in STATE_KEYS})
var_type = chain["var_type"]
for block in chain["blocks"]:
@@ -280,8 +330,10 @@ def estimate_sequential_fk(fk: RobotFK, obs: Dict[int, Dict[str, Any]], chain: D
bmarkers = [m for m in block["markers"] if m in obs]
if not bvars:
continue
if seed and all(v in seed for v in bvars):
continue # fully seeded — trust it, don't re-fit
if not bmarkers:
# unobservable block: leave at 0, flag later
# unobservable block: leave at seed/0, flag later
continue
if not HAVE_SCIPY:
@@ -458,7 +510,140 @@ def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict
return info
def estimate_pose(fk: RobotFK, obs: Dict[int, Dict[str, Any]], cfg: Dict[str, Any]) -> Dict[str, Any]:
# ==================================================================
# Mode: joint-origin calibration (--calibrate-origin)
# ==================================================================
def estimate_origin_calibration(fk: RobotFK, obs: Dict[int, Dict[str, Any]],
link_name: str, cfg: Dict[str, Any],
seed: Optional[Dict[str, float]] = None,
free_axes: Tuple[int, ...] = (1, 2)) -> Dict[str, Any]:
"""
Fit `link_name`'s OWN joint variable together with its
`jointToParent.origin` components (default: indices 1,2 = Y,Z) from that
link's own markers, in a single robust least-squares solve. All other
joint variables are held fixed at `seed` (or 0) — this assumes the rest of
the chain (in particular a slider `x` seed, if relevant) is already
trustworthy, same precondition as the existing geometric method.
Complements doc/Kalibrierung.md Schritt [4] ("Arm1 Y-Rotationsachse"),
which fits the axis from a dedicated 3-pose capture using marker *centres*
only (circle fit). This fits from a single capture's marker corner poses
(position + measured normal, same residual as estimate_pose), reusing
whatever Homing run data is already on hand instead of a separate capture
session — useful for ANY revolute/linear joint's origin, not just Arm1/y.
Never writes robot.json. `fk.links[link_name]["jointToParent"]["origin"]`
is mutated transiently during the solve (RobotFK.compute() re-reads it
fresh on every call — see robot_fk.py) and always restored before
returning, success or not.
Returns a report dict; result["status"] is one of:
"ok" | "scipy_missing" | "insufficient_markers" | "unknown_link" | "failed"
"""
if link_name not in fk.links:
return {"link": link_name, "status": "unknown_link"}
chain = analyze_chain(fk)
link_var = next((v for v, links in chain["var_links"].items() if link_name in links), None)
if link_var is None:
return {"link": link_name, "status": "unknown_link",
"detail": "link has no movable jointToParent"}
own_markers = [m for m in chain["link_markers"].get(link_name, []) if m in obs]
joint = fk.links[link_name].get("jointToParent", {}) or {}
origin = joint.get("origin", [0.0, 0.0, 0.0])
if not isinstance(origin, list):
origin = list(origin)
joint["origin"] = origin
origin_before = list(origin)
var_type = chain["var_type"].get(link_var, "linear")
result: Dict[str, Any] = {
"link": link_name, "joint_variable": link_var,
"joint_unit": "mm" if var_type == "linear" else "deg",
"origin_before_mm": origin_before, "free_axes": list(free_axes),
"n_markers": len(own_markers), "status": "skipped",
}
if not HAVE_SCIPY:
result["status"] = "scipy_missing"
return result
if len(own_markers) < 2:
result["status"] = "insufficient_markers"
return result
base = {k: 0.0 for k in STATE_KEYS}
if seed:
base.update({k: v for k, v in seed.items() if k in STATE_KEYS})
def fun(vec):
st = dict(base)
st[link_var] = vec[0]
for i, ax in enumerate(free_axes):
origin[ax] = vec[1 + i]
return residual_vector(st, fk, obs, own_markers, cfg)
starts = [base.get(link_var, 0.0)] if var_type != "revolute" else _multistart_values("revolute")
best, best_cost = None, float("inf")
try:
for a0 in starts:
vec0 = np.array([a0] + [origin_before[ax] for ax in free_axes], dtype=float)
try:
sol = least_squares(fun, vec0, loss=cfg.get("robust_loss", "huber"),
f_scale=float(cfg.get("huber_delta_mm", 8.0)),
max_nfev=int(cfg.get("max_iterations", 200)) * 3)
if sol.cost < best_cost:
best_cost, best = sol.cost, sol.x
except Exception:
continue
finally:
for ax in free_axes:
origin[ax] = origin_before[ax] # always restore — report-only tool
if best is None:
result["status"] = "failed"
return result
fitted_joint = float(best[0])
if var_type == "revolute":
fitted_joint = (fitted_joint + 180.0) % 360.0 - 180.0
fitted_origin = list(origin_before)
for i, ax in enumerate(free_axes):
fitted_origin[ax] = float(best[1 + i])
final_state = dict(base)
final_state[link_var] = fitted_joint
for i, ax in enumerate(free_axes):
origin[ax] = fitted_origin[ax]
final_res = residual_vector(final_state, fk, obs, own_markers, cfg)
for ax in free_axes:
origin[ax] = origin_before[ax] # restore again after the check above
result.update({
"status": "ok",
"joint_value": fitted_joint,
"origin_after_mm": fitted_origin,
"origin_delta_mm": [round(b - a, 4) for a, b in zip(origin_before, fitted_origin)],
"residual_rms": float(np.sqrt(np.mean(final_res ** 2))) if final_res.size else 0.0,
"note": "robot.json NOT modified — apply via Kalibrierung-Tab "
"\"Joint-Origin Y/Z übernehmen\" (editRobot.js) if this looks good.",
})
return result
def estimate_pose(fk: RobotFK, obs: Dict[int, Dict[str, Any]], cfg: Dict[str, Any],
seed: Optional[Dict[str, float]] = None) -> Dict[str, Any]:
"""
seed: optional partial/full joint state (e.g. from load_seed_state(), the
4b_revolute_angle.py chain) to trust as a starting point for global_ba/
hybrid. Passed through to estimate_sequential_fk(), which skips re-fitting
any block that is FULLY covered by seed and otherwise still applies its
normal per-block multi-start — so variables the seed does NOT cover keep
the existing local-minimum protection (see doc/Homing_5_Pose.md "Wichtige
Einschraenkung") instead of silently defaulting to an unprotected 0.
sequential_vector ignores seed (no x0 input; left untouched on purpose —
it is the cheap analytic method, not the one this seeding targets).
"""
chain = analyze_chain(fk)
var_names = chain["ordered_vars"]
method = str(cfg.get("method", "hybrid")).lower()
@@ -467,12 +652,9 @@ def estimate_pose(fk: RobotFK, obs: Dict[int, Dict[str, Any]], cfg: Dict[str, An
if method == "sequential_vector":
state = estimate_sequential_vector(fk, obs, chain, cfg)
elif method == "sequential_fk":
state = estimate_sequential_fk(fk, obs, chain, cfg)
elif method == "global_ba":
init = estimate_sequential_fk(fk, obs, chain, cfg) # cheap robust init
state = estimate_global_ba(fk, obs, var_names, init, cfg)
else: # hybrid (default)
init = estimate_sequential_fk(fk, obs, chain, cfg)
state = estimate_sequential_fk(fk, obs, chain, cfg, seed=seed)
else: # global_ba / hybrid (default) — both use the same init->refine path
init = estimate_sequential_fk(fk, obs, chain, cfg, seed=seed)
state = estimate_global_ba(fk, obs, var_names, init, cfg)
# final residual stats over all observed markers
@@ -493,6 +675,14 @@ def main() -> None:
ap.add_argument("-robot", "--robot", required=True)
ap.add_argument("-out", "--out", default=None)
ap.add_argument("--method", default=None, help="override robot.json method")
ap.add_argument("--from-state", default=None, metavar="JSON",
help="Seed/init state (flat {var:value} or {accumulated_state:{...}} as "
"written by 4b_revolute_angle.py). Used as x0 for global_ba/hybrid "
"instead of the internal cold start. See doc/Homing_5_Pose.md.")
ap.add_argument("--calibrate-origin", default=None, metavar="LINK",
help="Instead of estimating the full pose, fit LINK's own joint value "
"together with its jointToParent.origin Y/Z from LINK's own markers. "
"Writes a *_origin_calibration.json report; never modifies robot.json.")
args = ap.parse_args()
robot_data = json.load(open(args.robot, "r", encoding="utf-8"))
@@ -503,9 +693,31 @@ def main() -> None:
fk = RobotFK(robot_data)
obs = load_observations(args.markers, cfg.get("use_normals", True),
int(cfg.get("min_cameras_per_marker", 2)))
print(f"[INFO] method={cfg['method']} | observed markers={len(obs)} | use_normals={cfg.get('use_normals')}")
seed = load_seed_state(args.from_state) if args.from_state else None
print(f"[INFO] method={cfg['method']} | observed markers={len(obs)} | use_normals={cfg.get('use_normals')}"
+ (f" | seed={seed}" if seed else ""))
result = estimate_pose(fk, obs, cfg)
# ── Mode: joint-origin calibration ──────────────────────────────────────
if args.calibrate_origin:
calib = estimate_origin_calibration(fk, obs, args.calibrate_origin, cfg, seed=seed)
print(f"\nOrigin calibration for link={calib['link']} status={calib['status']}")
if calib["status"] == "ok":
unit = calib["joint_unit"]
print(f" joint {calib['joint_variable']}: {calib['joint_value']:.2f} {unit}")
print(f" origin before: {calib['origin_before_mm']}")
print(f" origin after: {calib['origin_after_mm']} (delta {calib['origin_delta_mm']} mm)")
print(f" residual RMS over {calib['n_markers']} markers: {calib['residual_rms']:.3f}")
print(f" {calib['note']}")
else:
print(f" (no fit — {calib.get('detail', calib['status'])}, n_markers={calib.get('n_markers', 0)})")
out_path = args.out or os.path.join(
os.path.dirname(args.markers), f"{args.calibrate_origin}_origin_calibration.json")
json.dump(calib, open(out_path, "w", encoding="utf-8"), indent=2)
print(f"[INFO] wrote {out_path}")
return
# ── Mode: full pose estimation (default) ────────────────────────────────
result = estimate_pose(fk, obs, cfg, seed=seed)
st = result["state"]
print("\nEstimated joint values:")
@@ -513,20 +725,28 @@ def main() -> None:
ob = result["observability"].get(v, {})
unit = "mm" if v in ("x", "e") else "deg"
conf = ob.get("confidence", "?")
tag = "" if ob.get("observable", False) else " [UNOBSERVABLE -> 0]"
tag = "" if ob.get("observable", False) else " [UNOBSERVABLE -> null]"
print(f" {v}: {st.get(v, 0.0):8.2f} {unit} (markers={ob.get('n_markers','?')}, conf={conf}){tag}")
print(f"\n[INFO] residual RMS over {result['num_markers']} markers: {result['residual_rms']:.3f}")
movements = {}
for v in ["x", "y", "z", "a", "b", "c", "e"]:
ob = result["observability"].get(v, {})
observable = ob.get("observable", False)
movements[v] = {
# Unobservable -> null, never a fabricated 0 (see module docstring).
"value": st.get(v, 0.0) if observable else None,
"unit": "mm" if v in ("x", "e") else "deg",
"observable": observable,
"confidence": ob.get("confidence", "none"),
"n_markers": ob.get("n_markers", 0),
}
out = {
"schema_version": "1.0",
"created_utc": time.strftime("%Y-%m-%dT%H:%M:%SZ", time.gmtime()),
"method": result["method"],
"movements": {v: {"value": st.get(v, 0.0),
"unit": "mm" if v in ("x", "e") else "deg",
"observable": result["observability"].get(v, {}).get("observable", False),
"confidence": result["observability"].get(v, {}).get("confidence", "none"),
"n_markers": result["observability"].get(v, {}).get("n_markers", 0)}
for v in ["x", "y", "z", "a", "b", "c", "e"]},
"seeded": seed is not None,
"movements": movements,
"residual_rms": result["residual_rms"],
"num_markers": result["num_markers"],
}