diff --git a/scripts/5_pose_estimation.py b/scripts/5_pose_estimation.py index 4d181aa..6c23805 100644 --- a/scripts/5_pose_estimation.py +++ b/scripts/5_pose_estimation.py @@ -230,11 +230,29 @@ def analyze_chain(fk: RobotFK) -> Dict[str, Any]: for x in pending: var_block[x] = len(blocks) - 1 + # subtree_markers[L] = L's own markers + all descendants' markers. Lets + # observability() credit a block whose own link saw nothing this capture + # but whose CHILD link did (e.g. Ellbow has no visible markers, but Arm2's + # markers still constrain z through the chain — same idea as 4b's + # Fallback-1, just for confidence reporting here, not for the fit itself). + children: Dict[str, List[str]] = defaultdict(list) + for ln, ld in links.items(): + p = ld.get("parent") + if p: + children[p].append(ln) + subtree_markers: Dict[str, List[int]] = {} + for ln in reversed(topo): + ids = list(link_markers.get(ln, [])) + for c in children.get(ln, []): + ids.extend(subtree_markers.get(c, [])) + subtree_markers[ln] = ids + return { "ordered_vars": ordered_vars, "var_type": var_type, "var_links": dict(var_links), "link_markers": link_markers, + "subtree_markers": subtree_markers, "blocks": blocks, } @@ -518,16 +536,25 @@ def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict driven by markers-per-variable in that block: high : >= 2 markers per variable (well over-determined) medium : >= 1 marker per variable - low : fewer markers than variables (under-determined — distrust!) - none : no markers at all (variable left at 0) + low : fewer markers than variables (under-determined — distrust!), + OR no own markers seen but a child link's markers were + (indirect evidence through the chain, e.g. Ellbow via Arm2) + none : no markers at all, not even indirectly (variable left at 0) """ info: Dict[str, Dict[str, Any]] = {} + subtree_markers = chain.get("subtree_markers", {}) for block in chain["blocks"]: seen = [m for m in block["markers"] if m in obs] + indirect = False + if not seen and block["anchor"]: + seen = [m for m in subtree_markers.get(block["anchor"], []) if m in obs] + indirect = bool(seen) nvars = max(1, len(block["vars"])) ratio = len(seen) / nvars if len(seen) == 0: conf = "none" + elif indirect: + conf = "low" # indirect/coupled through a child link, not direct elif ratio >= 2.0: conf = "high" elif ratio >= 1.0: @@ -537,7 +564,7 @@ def observability(chain: Dict[str, Any], obs: Dict[int, Dict[str, Any]]) -> Dict for v in block["vars"]: info[v] = {"observable": len(seen) > 0, "n_markers": len(seen), "block_vars": len(block["vars"]), "confidence": conf, - "block_anchor": block["anchor"]} + "block_anchor": block["anchor"], "indirect": indirect} return info diff --git a/scripts/__pycache__/5_pose_estimation.cpython-311.pyc b/scripts/__pycache__/5_pose_estimation.cpython-311.pyc index 63ef7df..eaf2566 100644 Binary files a/scripts/__pycache__/5_pose_estimation.cpython-311.pyc and b/scripts/__pycache__/5_pose_estimation.cpython-311.pyc differ diff --git a/scripts/robot_1781069752019.json b/scripts/robot_1781069752019.json index a9d922e..fa5031d 100644 --- a/scripts/robot_1781069752019.json +++ b/scripts/robot_1781069752019.json @@ -288,7 +288,7 @@ "name": "Joint1", "type": "revolute", "axis": [-1, 0, 0], - "origin": [110, 107.457, 37.242], + "origin": [110, 101.1, 55.2], "rotation": [0, 0, 0], "variable": "y" },