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

Axis automatisch

Browse Source
This commit is contained in:
chk
2026-06-16 17:36:46 +02:00
parent 5f6d28673a
commit eae6b6098a
6 changed files with 158 additions and 286 deletions
Download Patch File Download Diff File Expand all files Collapse all files

243
scripts/5_pose_estimation.py
View File

@@ -33,13 +33,15 @@ Homing integration (appRobotHoming, see doc/Homing_5_Pose.md):
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.
robot.json -> pose_estimation.fit_origin_link = "Arm1" one switch: the
named link's jointToParent.origin Y/Z is fit
TOGETHER WITH the normal pose in the same
global_ba solve (complements the geometric
multi-pose method in doc/Kalibrierung.md
Schritt [4]) and the result is adopted
automatically -- written back into robot.json
(surgical text patch, see patch_robot_json_origin()).
Off by default (key absent/null).
Unobservable joints (confidence "none") are written as value=null in the
output JSON — never a fabricated 0 (see movements.<var>.observable).
@@ -52,6 +54,7 @@ import argparse
import json
import math
import os
import re
import sys
import time
from collections import defaultdict
@@ -85,6 +88,12 @@ DEFAULT_CFG: Dict[str, Any] = {
"min_cameras_per_marker": 2,
"finger_block_joints": ["b", "c", "e"],
"per_link_method": {},
# One switch: if set to a link name (e.g. "Arm1"), that link's
# jointToParent.origin Y/Z is fit together with the normal pose (same
# global_ba solve) and the result is written back into robot.json
# automatically. None/absent = off, no behaviour change. See
# doc/Homing_5_Pose.md "Kalibrier-Switch".
"fit_origin_link": None,
}
@@ -277,20 +286,42 @@ def estimate_global_ba(fk: RobotFK, obs: Dict[int, Dict[str, Any]], var_names: L
marker_ids = list(obs.keys())
base = {k: 0.0 for k in STATE_KEYS}
base.update(x0)
vec0 = np.array([base.get(v, 0.0) for v in var_names], dtype=float)
# One switch (pose_estimation.fit_origin_link): also free that link's
# jointToParent.origin Y/Z as 2 extra parameters of THIS SAME solve, no
# separate pass. fk.links[...] is mutated in place -- compute() re-reads
# it fresh every call (robot_fk.py), so this takes effect immediately and
# is left adopted on success (main() writes it back into robot.json).
origin_link = cfg.get("fit_origin_link")
origin = fk.links.get(origin_link, {}).get("jointToParent", {}).get("origin") if origin_link else None
origin = origin if isinstance(origin, list) and len(origin) == 3 else None
origin_before = list(origin) if origin else None
n_state = len(var_names)
vec0 = np.array([base.get(v, 0.0) for v in var_names]
+ (origin_before[1:3] if origin else []), dtype=float)
def fun(vec):
st = _state_from_vec(var_names, vec, base)
st = _state_from_vec(var_names, vec[:n_state], base)
if origin:
origin[1], origin[2] = float(vec[n_state]), float(vec[n_state + 1])
return residual_vector(st, fk, obs, marker_ids, cfg)
loss = cfg.get("robust_loss", "huber")
f_scale = float(cfg.get("huber_delta_mm", 8.0))
try:
sol = least_squares(fun, vec0, loss=loss, f_scale=f_scale,
max_nfev=int(cfg.get("max_iterations", 200)) * max(1, len(var_names)))
return _state_from_vec(var_names, sol.x, base)
max_nfev=int(cfg.get("max_iterations", 200)) * max(1, len(vec0)))
state = _state_from_vec(var_names, sol.x[:n_state], base)
if origin:
origin[1], origin[2] = float(sol.x[n_state]), float(sol.x[n_state + 1])
print(f"[INFO] fit_origin_link={origin_link}: Y,Z {origin_before[1:3]} "
f"-> [{origin[1]:.3f}, {origin[2]:.3f}]")
return state
except Exception as exc:
print(f"[WARN] global_ba failed: {exc}")
if origin:
origin[1], origin[2] = origin_before[1], origin_before[2] # restore on failure
return dict(base)
@@ -510,127 +541,6 @@ def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict
return info
# ==================================================================
# 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]:
"""
@@ -669,6 +579,36 @@ def estimate_pose(fk: RobotFK, obs: Dict[int, Dict[str, Any]], cfg: Dict[str, An
# CLI
# ==================================================================
def patch_robot_json_origin(robot_path: str, link_name: str, yz: Tuple[float, float]) -> bool:
"""
Surgically rewrite links.<link_name>.jointToParent.origin[1],[2] (Y,Z) in
the robot.json TEXT in place. robot.json has a hand-curated, compact
format (markers etc. one per line) -- a full json.load()+json.dump()
round-trip would reformat the whole file, so this only touches the one
"origin": [...] array belonging to <link_name> (X left untouched).
Returns True if a match was found and patched.
"""
with open(robot_path, "r", encoding="utf-8") as f:
text = f.read()
link_m = re.search(r'"%s"\s*:\s*\{' % re.escape(link_name), text)
if not link_m:
return False
origin_m = re.search(r'"origin"\s*:\s*\[([^\]]*)\]', text[link_m.end():])
if not origin_m:
return False
parts = [p.strip() for p in origin_m.group(1).split(",")]
if len(parts) != 3:
return False
parts[1] = f"{float(yz[0]):.4f}".rstrip("0").rstrip(".")
parts[2] = f"{float(yz[1]):.4f}".rstrip("0").rstrip(".")
new_array = f"[{parts[0]}, {parts[1]}, {parts[2]}]"
start = link_m.end() + origin_m.start()
end = link_m.end() + origin_m.end()
with open(robot_path, "w", encoding="utf-8") as f:
f.write(text[:start] + '"origin": ' + new_array + text[end:])
return True
def main() -> None:
ap = argparse.ArgumentParser(description="Estimate robot joint angles from marker poses")
ap.add_argument("markers", help="aruco_marker_poses.json (corner_pose) or aruco_positions_*.json (center)")
@@ -679,10 +619,6 @@ def main() -> None:
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"))
@@ -696,27 +632,9 @@ def main() -> None:
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 ""))
if cfg.get("fit_origin_link"):
print(f"[INFO] fit_origin_link={cfg['fit_origin_link']} -> robot.json wird bei Erfolg aktualisiert")
# ── 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"]
@@ -754,6 +672,19 @@ def main() -> None:
json.dump(out, open(out_path, "w", encoding="utf-8"), indent=2)
print(f"[INFO] wrote {out_path}")
# "Uebernehmen": fit_origin_link's Y/Z is already adopted on `fk` (see
# estimate_global_ba) -- write it back into robot.json itself.
origin_link = cfg.get("fit_origin_link")
if origin_link:
origin = fk.links.get(origin_link, {}).get("jointToParent", {}).get("origin")
if isinstance(origin, list) and len(origin) == 3:
if patch_robot_json_origin(args.robot, origin_link, (origin[1], origin[2])):
print(f"[INFO] robot.json aktualisiert: {origin_link}.jointToParent.origin "
f"= [{origin[0]}, {origin[1]:.3f}, {origin[2]:.3f}]")
else:
print(f"[WARN] {origin_link}.jointToParent.origin in {args.robot} nicht gefunden — "
f"nicht aktualisiert (Wert nur in {out_path} sichtbar)")
if __name__ == "__main__":
main()

BIN
scripts/__pycache__/5_pose_estimation.cpython-311.pyc
View File

Binary file not shown.

3
scripts/robot_1781069752019.json
View File

@@ -98,7 +98,8 @@
"max_iterations": 200,
"min_cameras_per_marker": 2,
"finger_block_joints": ["b", "c", "e"],
"per_link_method": {}
"per_link_method": {},
"fit_origin_link": "Arm1"
},
"robot_test_poses": {
"4": {"x": 70, "y": 50, "z": -70, "a": 120, "b": 50, "c": 30, "e": 20},