File size: 3,750 Bytes
d8290eb | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 | """Audited suspended-settle diagnostics for the pre-policy frozen pose."""
from __future__ import annotations
import numpy as np
from .h2_contract import H2_DOF, HARDWARE_JOINT_NAMES
# Only shoulder and elbow joints use the wider frozen-pose diagnostic band. The
# 2026-09-04 suspended commissioning run measured a stable 0.442 rad arm error
# with the ONNX arm stiffness (14.25 Nm/rad); fixed-base inverse dynamics gives
# 0.34--0.41 rad static offsets for these joints. Wrists, torso, legs and head
# remain on the common diagnostic band. Position error is not an L1 admission
# gate, matching the G1 deployment's direct policy start semantics. The tether
# can create large static PD offsets before the balance policy begins.
FROZEN_GRAVITY_LOADED_ARM_IDS = (15, 16, 17, 18, 22, 23, 24, 25)
def position_tolerances_hw(common_rad: float, arm_rad: float) -> np.ndarray:
tolerance = np.full(H2_DOF, float(common_rad), dtype=np.float64)
tolerance[list(FROZEN_GRAVITY_LOADED_ARM_IDS)] = float(arm_rad)
if not np.isfinite(tolerance).all() or np.any(tolerance <= 0.0):
raise ValueError("frozen-pose position tolerances must be finite and positive")
return tolerance
def tracking_status(
q_hw: np.ndarray,
ready_q_hw: np.ndarray,
tolerance_hw: np.ndarray,
) -> tuple[bool, float, int, np.ndarray]:
q = np.asarray(q_hw, dtype=np.float64).reshape(H2_DOF)
ready = np.asarray(ready_q_hw, dtype=np.float64).reshape(H2_DOF)
tolerance = np.asarray(tolerance_hw, dtype=np.float64).reshape(H2_DOF)
if not np.isfinite(q).all() or not np.isfinite(ready).all():
raise ValueError("frozen-pose tracking inputs must be finite")
error = np.abs(q - ready)
normalized = error / tolerance
worst_id = int(np.argmax(normalized))
return bool(np.all(normalized <= 1.0)), float(error[worst_id]), worst_id, error
def suspended_settle_status(
q_hw: np.ndarray,
dq_hw: np.ndarray,
ready_q_hw: np.ndarray,
tolerance_hw: np.ndarray,
velocity_tolerance_rad_s: float,
) -> tuple[bool, bool, float, int, float]:
"""Require a quiet finite suspension state; report pose error without gating it."""
q = np.asarray(q_hw, dtype=np.float64).reshape(H2_DOF)
dq = np.asarray(dq_hw, dtype=np.float64).reshape(H2_DOF)
if not np.isfinite(q).all() or not np.isfinite(dq).all():
raise ValueError("suspended-settle state must be finite")
velocity_limit = float(velocity_tolerance_rad_s)
if not np.isfinite(velocity_limit) or velocity_limit <= 0.0:
raise ValueError("suspended-settle velocity tolerance must be finite and positive")
position_ok, worst_error, worst_id, _ = tracking_status(
q, ready_q_hw, tolerance_hw
)
velocity_error = float(np.max(np.abs(dq)))
return (
velocity_error <= velocity_limit,
position_ok,
worst_error,
worst_id,
velocity_error,
)
def error_summary(
q_hw: np.ndarray,
ready_q_hw: np.ndarray,
tolerance_hw: np.ndarray,
*,
limit: int = 4,
) -> str:
q = np.asarray(q_hw, dtype=np.float64).reshape(H2_DOF)
ready = np.asarray(ready_q_hw, dtype=np.float64).reshape(H2_DOF)
tolerance = np.asarray(tolerance_hw, dtype=np.float64).reshape(H2_DOF)
error = np.abs(q - ready)
order = np.argsort(-(error / tolerance))[: max(1, int(limit))]
return ", ".join(
f"id={index:02d}:{HARDWARE_JOINT_NAMES[index]} "
f"q={q[index]:+.3f} target={ready[index]:+.3f} "
f"err={error[index]:.3f}/{tolerance[index]:.3f}rad"
for index in order
)
__all__ = [
"FROZEN_GRAVITY_LOADED_ARM_IDS",
"error_summary",
"position_tolerances_hw",
"suspended_settle_status",
"tracking_status",
]
|