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

4b_revolute mit Fallback

Browse Source
This commit is contained in:
chk
2026-06-16 14:52:18 +02:00
parent ad208b7d21
commit 0234c1ef1d
17 changed files with 14069 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

204
scripts/4b_revolute_angle.py
View File

@@ -5,11 +5,17 @@
Generic revolute-joint angle estimator.
For each movable link (Arm1, Ellbow, Arm2 …) whose joint type is 'revolute',
this script estimates the rotation angle using the pairwise-vector method:
this script estimates the rotation angle using the pairwise-vector method
(PRIMARY), with a single-marker pivot method as FALLBACK:
For every PAIR (m1, m2) of markers belonging to the target link:
v_model = local_pos_m2 - local_pos_m1 (in link's own frame)
v_obs = world_pos_m2 - world_pos_m1 (in world frame)
PRIMARY — for every PAIR (m1, m2) of markers belonging to the target link:
v_model = spoke_world(m2) - spoke_world(m1) (model, world-oriented)
v_obs = world_pos_m2 - world_pos_m1 (observed, world frame)
where spoke_world(m) rotates the marker's local `position` (from
robot.json) through the already-known PARENT joints via FK, so it is
expressed in world orientation at this joint's own angle = 0 — exactly
the frame v_obs lives in.
Both vectors are projected perpendicular to the joint axis (in world frame),
and the signed angle from v_model_perp to v_obs_perp is measured.
@@ -20,6 +26,16 @@ this script estimates the rotation angle using the pairwise-vector method:
Pair weights = baseline_model × baseline_obs (longer baselines → more reliable).
FALLBACK — only used when the PRIMARY method has no usable pair at all
(e.g. just one marker visible, or every visible pair happens to lie
parallel to the joint axis, as for two markers spaced along a forearm):
the joint PIVOT itself stands in for the missing second marker, i.e. the
"pair" becomes (pivot, m1). This needs only ONE matched marker, but —
unlike the primary method — its accuracy additionally depends on the
already-estimated PARENT joint *values* being correct (not just their
axis direction), since the pivot's world position comes from FK. See
`PIVOT_FALLBACK_ID` / `used_fallback` in the code.
How to use sequentially
-----------------------
Run 4b once per revolute joint, from root to tip:
@@ -36,6 +52,7 @@ Output JSON
{
"link": "Arm1",
"joint": "y",
"method": "marker_pair", // or "pivot_fallback" — see FALLBACK above
"mean_angle_deg": 86.3,
"circular_std_deg": 0.7,
"num_pairs": 6,
@@ -62,6 +79,12 @@ from robot_fk import RobotFK
STATE_KEYS = ("x", "y", "z", "a", "b", "c", "e")
# Sentinel "marker id" used in `per_pair` reports for the joint pivot.
# Only ever appears when the FALLBACK path (pivot vs. a single marker)
# was used instead of a real marker-to-marker pair — see the
# `used_fallback` block inside `estimate_revolute_angle()` below.
PIVOT_FALLBACK_ID = -1
# ──────────────────────────────────────────────────────────────
# I/O
@@ -130,6 +153,78 @@ def _circular_mean_deg(angles_rad: np.ndarray,
return math.degrees(mean), c_var, math.degrees(c_std)
# ──────────────────────────────────────────────────────────────
# Shared spoke/pair math
# (used by BOTH the primary marker-pair method and the pivot FALLBACK)
# ──────────────────────────────────────────────────────────────
def _model_spoke_world(fk: RobotFK,
zero_transforms: Dict[str, np.ndarray],
link_name: str,
origin_world: np.ndarray,
local_pos: np.ndarray) -> np.ndarray:
"""
Vector from the joint PIVOT to a marker, in WORLD ORIENTATION, as it
would be if this joint's own angle were 0 (only the already-known
PARENT rotations applied).
This is what `v_model` must be expressed as before comparing it to
`axis_world` / an observed vector: the raw `position` field from
robot.json lives in the link's own pre-rotation local frame and is
NOT yet rotated into world orientation by the parent chain. Skipping
this rotation silently biases the estimated angle by (roughly) the
parent joint's own angle — invisible whenever the parent chain has
no rotation yet (e.g. Arm1, whose parent 'Base' is purely linear),
but wrong for anything further down the chain (Ellbow, Arm2, Hand …).
"""
p0 = fk.marker_world(zero_transforms, link_name, local_pos)
return p0 - origin_world
def _pair_estimate(v_model: np.ndarray,
v_obs: np.ndarray,
axis_world: np.ndarray,
marker_ids: Tuple[int, int],
min_baseline_mm: float,
fallback: bool) -> Tuple[Optional[float], Optional[float], dict]:
"""
Project model/observed vectors perpendicular to the joint axis and
derive one angle estimate from them. Returns (angle_rad, weight,
per_pair_entry) — angle_rad/weight are None when skipped (baseline
too short).
`fallback=True` marks entries produced by the pivot FALLBACK (one of
the two "markers" is actually the joint pivot, see PIVOT_FALLBACK_ID)
so callers/reports can always tell primary and fallback data apart.
"""
v_model_perp = _project_perp(v_model, axis_world)
v_obs_perp = _project_perp(v_obs, axis_world)
bl_model = float(np.linalg.norm(v_model_perp))
bl_obs = float(np.linalg.norm(v_obs_perp))
if bl_model < min_baseline_mm or bl_obs < min_baseline_mm:
return None, None, {
"marker_ids": list(marker_ids),
"fallback": fallback,
"skipped": True,
"reason": f"bl_model={bl_model:.1f} bl_obs={bl_obs:.1f} < {min_baseline_mm}",
}
angle = _wrap(_signed_angle_rad(v_model_perp, v_obs_perp, axis_world))
weight = bl_model * bl_obs
entry = {
"marker_ids": list(marker_ids),
"fallback": fallback,
"skipped": False,
"angle_deg": math.degrees(angle),
"baseline_model_mm": bl_model,
"baseline_obs_mm": bl_obs,
"weight": weight,
}
return angle, weight, entry
# ──────────────────────────────────────────────────────────────
# Core estimator (generic — works for any revolute joint)
# ──────────────────────────────────────────────────────────────
@@ -179,8 +274,9 @@ def estimate_revolute_angle(
zero_state = dict(known_state)
zero_state[var] = 0.0
origin_world = fk.joint_origin_world(link_name, zero_state)
axis_world = fk.joint_axis_world(link_name, zero_state)
origin_world = fk.joint_origin_world(link_name, zero_state)
axis_world = fk.joint_axis_world(link_name, zero_state)
zero_transforms = fk.compute(zero_state) # needed to rotate model spokes into world orientation
# ── collect matched markers ───────────────────────────────
model_local: Dict[int, np.ndarray] = {}
@@ -192,15 +288,24 @@ def estimate_revolute_angle(
matched = {mid: (model_local[mid], observed_mm[mid])
for mid in model_local if mid in observed_mm}
if len(matched) < 2:
# Only 1 matched marker is enough to *attempt* an estimate — the
# PIVOT FALLBACK below can work with a single marker. With 0 there
# is nothing to go on at all.
if len(matched) < 1:
return {
"status": "failed",
"reason": (f"Need ≥2 matched markers, found {len(matched)}: "
f"{list(matched.keys())}. "
"reason": (f"Need ≥1 matched marker, found {len(matched)}. "
f"Model marker IDs: {list(model_local.keys())}"),
}
# ── pairwise estimation ───────────────────────────────────
def _spoke(local_pos: np.ndarray) -> np.ndarray:
return _model_spoke_world(fk, zero_transforms, link_name, origin_world, local_pos)
# ── PRIMARY: marker-to-marker pairs within this link ──────
# Preferred whenever ≥2 markers with a usable (non axis-parallel)
# baseline are visible. Only the AXIS DIRECTION needs to be correct
# for this — not the pivot's position — so it is the more robust
# source of truth and is always tried first.
ids = sorted(matched.keys())
angle_rad_list: List[float] = []
weight_list: List[float] = []
@@ -210,42 +315,49 @@ def estimate_revolute_angle(
l1, o1 = matched[id1]
l2, o2 = matched[id2]
v_model = l2 - l1 # local frame, both in same link
v_obs = o2 - o1 # world frame
v_model = _spoke(l2) - _spoke(l1) # model, world-oriented
v_obs = o2 - o1 # observed, world frame
v_model_perp = _project_perp(v_model, axis_world)
v_obs_perp = _project_perp(v_obs, axis_world)
angle, weight, entry = _pair_estimate(
v_model, v_obs, axis_world, (id1, id2), min_baseline_mm, fallback=False)
per_pair.append(entry)
if angle is not None:
angle_rad_list.append(angle)
weight_list.append(weight)
bl_model = float(np.linalg.norm(v_model_perp))
bl_obs = float(np.linalg.norm(v_obs_perp))
# ── FALLBACK: pivot + single marker, axis from predecessor ────
# Only entered when the PRIMARY method above produced NOT A SINGLE
# usable pair (e.g. only one marker visible at all, or every visible
# pair happens to lie parallel to the joint axis — as for two
# markers spaced along a forearm). Each matched marker is paired
# with the joint PIVOT instead of another marker, using the
# rotation axis already known from the predecessor joints.
# This is strictly a fallback: compared to a real 2-marker baseline
# it additionally relies on the predecessor joints' *values* (not
# just their axis direction) being accurate, since the pivot's
# world position is computed via FK rather than observed directly.
used_fallback = False
if not angle_rad_list:
used_fallback = True
for mid in ids:
l, o = matched[mid]
v_model = _spoke(l) # pivot → marker, model, world-oriented
v_obs = o - origin_world # pivot → marker, observed
if bl_model < min_baseline_mm or bl_obs < min_baseline_mm:
per_pair.append({
"marker_ids": [id1, id2],
"skipped": True,
"reason": f"bl_model={bl_model:.1f} bl_obs={bl_obs:.1f} < {min_baseline_mm}",
})
continue
angle = _wrap(_signed_angle_rad(v_model_perp, v_obs_perp, axis_world))
w = bl_model * bl_obs
angle_rad_list.append(angle)
weight_list.append(w)
per_pair.append({
"marker_ids": [id1, id2],
"skipped": False,
"angle_deg": math.degrees(angle),
"baseline_model_mm": bl_model,
"baseline_obs_mm": bl_obs,
"weight": w,
})
angle, weight, entry = _pair_estimate(
v_model, v_obs, axis_world,
(PIVOT_FALLBACK_ID, mid), min_baseline_mm, fallback=True)
per_pair.append(entry)
if angle is not None:
angle_rad_list.append(angle)
weight_list.append(weight)
if not angle_rad_list:
return {
"status": "failed",
"reason": "All pairs below min_baseline_mm. "
"Try --min-baseline 5 or check step-3 output.",
"reason": "All pairs below min_baseline_mm, including the "
"pivot fallback. Try --min-baseline 5 or check "
"step-3 output.",
}
mean_deg, c_var, c_std_deg = _circular_mean_deg(
@@ -258,16 +370,23 @@ def estimate_revolute_angle(
print(f" Joint origin (world): [{', '.join(f'{v:.1f}' for v in origin_world)}] mm")
print(f" Joint axis (world): [{', '.join(f'{v:.3f}' for v in axis_world)}]")
print(f" Matched markers: {list(matched.keys())}")
print(f" Pairs used: {len(angle_rad_list)} / {len(list(combinations(ids, 2)))}")
if used_fallback:
print(f" [FALLBACK] No usable marker-marker pair — estimating from "
f"pivot + predecessor axis instead (single-marker spokes).")
print(f" Pairs used: {len(angle_rad_list)} / {len(per_pair)}")
print(f" Angle: {mean_deg:+.2f} ° circular_σ {c_std_deg:.2f} °")
if c_std_deg > 5.0:
print(f" [WARN] high spread – step-3 errors or marker overlap")
print(f"\n Pair detail:")
for pp in per_pair:
id0, id1_ = pp["marker_ids"]
m0 = "PIVOT" if id0 == PIVOT_FALLBACK_ID else f"M{id0}"
m1 = "PIVOT" if id1_ == PIVOT_FALLBACK_ID else f"M{id1_}"
tag = " [fallback]" if pp.get("fallback") else ""
if pp["skipped"]:
print(f" M{pp['marker_ids'][0]}↔M{pp['marker_ids'][1]}: SKIPPED – {pp['reason']}")
print(f" {m0}↔{m1}{tag}: SKIPPED – {pp['reason']}")
else:
print(f" M{pp['marker_ids'][0]}↔M{pp['marker_ids'][1]}: "
print(f" {m0}↔{m1}{tag}: "
f"{pp['angle_deg']:+7.2f}° "
f"bl_model={pp['baseline_model_mm']:.1f} "
f"bl_obs={pp['baseline_obs_mm']:.1f}")
@@ -280,6 +399,7 @@ def estimate_revolute_angle(
"status": "ok",
"link": link_name,
"joint": var,
"method": "pivot_fallback" if used_fallback else "marker_pair",
"joint_origin_world_mm": origin_world.tolist(),
"joint_axis_world": axis_world.tolist(),
"mean_angle_deg": mean_deg,