| """Safety-critical H2 policy, joint-order and actuator contract. |
| |
| There are three different orders in the source material: |
| |
| * the ONNX policy uses Isaac Lab's breadth-first articulation order; |
| * the H2 MJCF/ready-pose files are name-indexed and use their own orders; |
| * Unitree ``unitree_hg`` LowCmd/LowState uses the official 0..30 IDL order. |
| |
| Nothing in this module relies on incidental array position. Every mapping is |
| built from exact names and checked against the published H2 PR-mode contract. |
| """ |
|
|
| from __future__ import annotations |
|
|
| from dataclasses import dataclass |
| from pathlib import Path |
| import xml.etree.ElementTree as ET |
|
|
| import numpy as np |
|
|
| from .observation import expected_obs_dim |
| from .policy import OnnxPolicy |
|
|
|
|
| H2_DOF = 31 |
| LOWCMD_DOF = 35 |
| MODE_PR = 0 |
| |
| |
| COMMAND_JOINT_MARGIN_RAD = 0.01 |
|
|
| |
| |
| |
| |
| |
| |
| H2_JOINT_MAP_REVISION = "unitree_sdk2@e02dc701-h2-cpp" |
|
|
| |
| |
| |
| |
| HARDWARE_JOINT_NAMES: tuple[str, ...] = ( |
| "left_hip_pitch_joint", |
| "left_hip_roll_joint", |
| "left_hip_yaw_joint", |
| "left_knee_joint", |
| "left_ankle_roll_joint", |
| "left_ankle_pitch_joint", |
| "right_hip_pitch_joint", |
| "right_hip_roll_joint", |
| "right_hip_yaw_joint", |
| "right_knee_joint", |
| "right_ankle_roll_joint", |
| "right_ankle_pitch_joint", |
| "waist_yaw_joint", |
| "waist_roll_joint", |
| "waist_pitch_joint", |
| "left_shoulder_pitch_joint", |
| "left_shoulder_roll_joint", |
| "left_shoulder_yaw_joint", |
| "left_elbow_joint", |
| "left_wrist_roll_joint", |
| "left_wrist_pitch_joint", |
| "left_wrist_yaw_joint", |
| "right_shoulder_pitch_joint", |
| "right_shoulder_roll_joint", |
| "right_shoulder_yaw_joint", |
| "right_elbow_joint", |
| "right_wrist_roll_joint", |
| "right_wrist_pitch_joint", |
| "right_wrist_yaw_joint", |
| "head_pitch_joint", |
| "head_yaw_joint", |
| ) |
|
|
| |
| EXPECTED_POLICY_JOINT_NAMES: tuple[str, ...] = ( |
| "left_hip_pitch_joint", |
| "right_hip_pitch_joint", |
| "waist_yaw_joint", |
| "left_hip_roll_joint", |
| "right_hip_roll_joint", |
| "waist_roll_joint", |
| "left_hip_yaw_joint", |
| "right_hip_yaw_joint", |
| "waist_pitch_joint", |
| "left_knee_joint", |
| "right_knee_joint", |
| "head_pitch_joint", |
| "left_shoulder_pitch_joint", |
| "right_shoulder_pitch_joint", |
| "left_ankle_roll_joint", |
| "right_ankle_roll_joint", |
| "head_yaw_joint", |
| "left_shoulder_roll_joint", |
| "right_shoulder_roll_joint", |
| "left_ankle_pitch_joint", |
| "right_ankle_pitch_joint", |
| "left_shoulder_yaw_joint", |
| "right_shoulder_yaw_joint", |
| "left_elbow_joint", |
| "right_elbow_joint", |
| "left_wrist_roll_joint", |
| "right_wrist_roll_joint", |
| "left_wrist_pitch_joint", |
| "right_wrist_pitch_joint", |
| "left_wrist_yaw_joint", |
| "right_wrist_yaw_joint", |
| ) |
|
|
| |
| |
| EXPECTED_HW_TO_POLICY: tuple[int, ...] = ( |
| 0, 3, 6, 9, 14, 19, |
| 1, 4, 7, 10, 15, 20, |
| 2, 5, 8, |
| 12, 17, 21, 23, 25, 27, 29, |
| 13, 18, 22, 24, 26, 28, 30, |
| 11, 16, |
| ) |
| EXPECTED_POLICY_TO_HW: tuple[int, ...] = ( |
| 0, 6, 12, 1, 7, 13, 2, 8, 14, 3, 9, 29, |
| 15, 22, 4, 10, 30, 16, 23, 5, 11, 17, 24, |
| 18, 25, 19, 26, 20, 27, 21, 28, |
| ) |
|
|
| FIXED_HARDWARE_TARGETS = {29: 0.0, 30: 0.0} |
| EXPECTED_OBSERVATION_NAMES: tuple[str, ...] = ( |
| "task_command", |
| "motion_command", |
| "root_current_xy", |
| "motion_global_xy", |
| "motion_root_ori", |
| "projected_gravity", |
| "root_heading_xy", |
| "base_ang_vel", |
| "joint_pos", |
| "joint_vel", |
| "actions", |
| ) |
|
|
|
|
| def _csv_floats(raw: object, field: str) -> np.ndarray: |
| if raw is None: |
| raise RuntimeError(f"ONNX metadata is missing {field!r}") |
| if isinstance(raw, str): |
| values = [float(item.strip()) for item in raw.split(",") if item.strip()] |
| else: |
| values = [float(item) for item in raw] |
| result = np.asarray(values, dtype=np.float64) |
| if result.shape != (H2_DOF,) or not np.isfinite(result).all(): |
| raise RuntimeError(f"ONNX metadata {field!r} must be {H2_DOF} finite values, got {result.shape}") |
| return result |
|
|
|
|
| def _validated_command_joint_margin(value: float) -> float: |
| margin = float(value) |
| if not np.isfinite(margin) or margin < COMMAND_JOINT_MARGIN_RAD: |
| raise ValueError( |
| "joint_margin_rad must be finite and at least the H2 bridge margin " |
| f"{COMMAND_JOINT_MARGIN_RAD:.2f}rad" |
| ) |
| return margin |
|
|
|
|
| def command_position_bounds_hw( |
| joint_lo_hw: np.ndarray, |
| joint_hi_hw: np.ndarray, |
| *, |
| joint_margin_rad: float = COMMAND_JOINT_MARGIN_RAD, |
| ) -> tuple[np.ndarray, np.ndarray]: |
| """Return the exact float32 position interval evaluated by the C++ bridge. |
| |
| H2BR carries float32 commands and production computes |
| ``kJointLo[index] + kJointMargin`` using ``float`` constants. Computing the |
| same decimal expression in float64 can differ by one float32 ULP and reject |
| a target that production accepts exactly on the boundary. |
| """ |
|
|
| margin = np.float32(_validated_command_joint_margin(joint_margin_rad)) |
| lo = np.asarray(joint_lo_hw, dtype=np.float32) + margin |
| hi = np.asarray(joint_hi_hw, dtype=np.float32) - margin |
| if lo.shape != (H2_DOF,) or hi.shape != (H2_DOF,): |
| raise ValueError(f"H2 position bounds must each have shape ({H2_DOF},)") |
| if not np.isfinite(lo).all() or not np.isfinite(hi).all() or np.any(lo >= hi): |
| raise RuntimeError("joint-limit margin collapses an H2 joint range") |
| return lo, hi |
|
|
|
|
| def _parse_mjcf_limits(path: str | Path) -> tuple[dict[str, tuple[float, float]], dict[str, float]]: |
| root = ET.parse(Path(path)).getroot() |
| ranges: dict[str, tuple[float, float]] = {} |
| torque: dict[str, float] = {} |
| for joint in root.iter("joint"): |
| name = joint.get("name") |
| if name not in HARDWARE_JOINT_NAMES: |
| continue |
| text = joint.get("range") |
| if text is None: |
| raise RuntimeError(f"MJCF joint {name} has no range") |
| lo, hi = (float(item) for item in text.split()) |
| if not np.isfinite([lo, hi]).all() or not lo < hi: |
| raise RuntimeError(f"invalid MJCF range for {name}: {text!r}") |
| ranges[name] = (lo, hi) |
| force = joint.get("actuatorfrcrange") |
| if force is not None: |
| f0, f1 = (float(item) for item in force.split()) |
| torque[name] = max(abs(f0), abs(f1)) |
|
|
| |
| for actuator in root.iter("actuator"): |
| for motor in actuator: |
| name = motor.get("joint") |
| if name not in HARDWARE_JOINT_NAMES or name in torque: |
| continue |
| text = motor.get("ctrlrange") |
| if text: |
| f0, f1 = (float(item) for item in text.split()) |
| torque[name] = max(abs(f0), abs(f1)) |
|
|
| missing_ranges = sorted(set(HARDWARE_JOINT_NAMES) - set(ranges)) |
| missing_torque = sorted(set(HARDWARE_JOINT_NAMES) - set(torque)) |
| if missing_ranges or missing_torque: |
| raise RuntimeError( |
| f"MJCF contract incomplete: missing ranges={missing_ranges}, torque={missing_torque}" |
| ) |
| return ranges, torque |
|
|
|
|
| @dataclass(frozen=True) |
| class H2Contract: |
| policy_names: tuple[str, ...] |
| policy_to_hw: np.ndarray |
| hw_to_policy: np.ndarray |
| default_policy: np.ndarray |
| action_scale_policy: np.ndarray |
| kp_policy: np.ndarray |
| kd_policy: np.ndarray |
| kp_hw: np.ndarray |
| kd_hw: np.ndarray |
| joint_lo_hw: np.ndarray |
| joint_hi_hw: np.ndarray |
| torque_hw: np.ndarray |
|
|
| @classmethod |
| def from_policy(cls, policy: OnnxPolicy, mjcf_path: str | Path) -> "H2Contract": |
| if policy.obs_dim != expected_obs_dim(H2_DOF) or policy.act_dim != H2_DOF: |
| raise RuntimeError( |
| f"H2 policy must be obs={expected_obs_dim(H2_DOF)}/actions={H2_DOF}, " |
| f"got {policy.obs_dim}/{policy.act_dim}" |
| ) |
| names = tuple(policy.metadata.joint_names) |
| if names != EXPECTED_POLICY_JOINT_NAMES: |
| raise RuntimeError( |
| "H2 ONNX joint order differs from the audited deployment contract; " |
| "refuse to infer a new real-robot mapping" |
| ) |
| if len(set(names)) != H2_DOF or set(names) != set(HARDWARE_JOINT_NAMES): |
| raise RuntimeError("policy and H2 hardware joint-name sets are not an exact 31-joint match") |
| if tuple(policy.metadata.observation_names) != EXPECTED_OBSERVATION_NAMES: |
| raise RuntimeError( |
| f"unexpected H2 observation metadata: {policy.metadata.observation_names}" |
| ) |
|
|
| policy_to_hw = np.asarray([HARDWARE_JOINT_NAMES.index(name) for name in names], dtype=np.int64) |
| hw_to_policy = np.asarray([names.index(name) for name in HARDWARE_JOINT_NAMES], dtype=np.int64) |
| if tuple(policy_to_hw.tolist()) != EXPECTED_POLICY_TO_HW: |
| raise RuntimeError(f"policy_to_hw tripwire failed: {policy_to_hw.tolist()}") |
| if tuple(hw_to_policy.tolist()) != EXPECTED_HW_TO_POLICY: |
| raise RuntimeError(f"hw_to_policy tripwire failed: {hw_to_policy.tolist()}") |
| probe = np.arange(H2_DOF) |
| if not np.array_equal(probe, probe[policy_to_hw][hw_to_policy]): |
| raise RuntimeError("H2 policy/hardware joint mapping is not a round-trip bijection") |
|
|
| default = np.asarray(policy.metadata.default_joint_pos, dtype=np.float64) |
| scale = np.asarray(policy.metadata.action_scale, dtype=np.float64) |
| if ( |
| default.shape != (H2_DOF,) |
| or scale.shape != (H2_DOF,) |
| or not np.isfinite(default).all() |
| or not np.isfinite(scale).all() |
| ): |
| raise RuntimeError("H2 policy default_joint_pos/action_scale metadata must each have 31 values") |
| kp = _csv_floats(policy.metadata.raw.get("joint_stiffness"), "joint_stiffness") |
| kd = _csv_floats(policy.metadata.raw.get("joint_damping"), "joint_damping") |
| if np.any(scale <= 0.0) or np.any(kp <= 0.0) or np.any(kd < 0.0): |
| raise RuntimeError("action scales and PD gains violate the H2 positive-value contract") |
|
|
| ranges, torque = _parse_mjcf_limits(mjcf_path) |
| lo_hw = np.asarray([ranges[name][0] for name in HARDWARE_JOINT_NAMES], dtype=np.float64) |
| hi_hw = np.asarray([ranges[name][1] for name in HARDWARE_JOINT_NAMES], dtype=np.float64) |
| torque_hw = np.asarray([torque[name] for name in HARDWARE_JOINT_NAMES], dtype=np.float64) |
| kp_hw = kp[hw_to_policy] |
| kd_hw = kd[hw_to_policy] |
|
|
| return cls( |
| policy_names=names, |
| policy_to_hw=policy_to_hw, |
| hw_to_policy=hw_to_policy, |
| default_policy=default, |
| action_scale_policy=scale, |
| kp_policy=kp, |
| kd_policy=kd, |
| kp_hw=kp_hw, |
| kd_hw=kd_hw, |
| joint_lo_hw=lo_hw, |
| joint_hi_hw=hi_hw, |
| torque_hw=torque_hw, |
| ) |
|
|
| def policy_from_hw(self, values_hw: np.ndarray) -> np.ndarray: |
| values = np.asarray(values_hw, dtype=np.float64).reshape(H2_DOF) |
| return values[self.policy_to_hw] |
|
|
| def hw_from_policy(self, values_policy: np.ndarray) -> np.ndarray: |
| values = np.asarray(values_policy, dtype=np.float64).reshape(H2_DOF) |
| return values[self.hw_to_policy] |
|
|
| def ready_hw(self, ready_policy: np.ndarray) -> np.ndarray: |
| result = self.hw_from_policy(ready_policy) |
| for index, value in FIXED_HARDWARE_TARGETS.items(): |
| result[index] = value |
| return result.astype(np.float32) |
|
|
| def learned_raw_hw_targets_from_action( |
| self, |
| applied_action_policy: np.ndarray, |
| ) -> np.ndarray: |
| """Map an applied policy action to the raw learned PD target in HW order. |
| |
| ``applied_action_policy`` is the action after the training-time |
| ``[-10, 10]`` action clip. The resulting position is deliberately a |
| *virtual* PD target: it is not clipped to the mechanical joint range or |
| projected against a state sampled at 50 Hz. This matches the working |
| sim2sim/G1 actuator contract, where effort saturation is applied by the |
| physics/1000 Hz actuator layer using its latest state. |
| |
| H2 head joints remain fixed deployment intents. Their zero targets are |
| written after the name-based policy-to-hardware mapping, without |
| changing the action retained in the observation history. |
| """ |
|
|
| action = np.asarray(applied_action_policy, dtype=np.float64).reshape(H2_DOF) |
| if not np.isfinite(action).all(): |
| raise RuntimeError("non-finite H2 policy action") |
| desired_policy = self.default_policy + self.action_scale_policy * action |
| desired_hw = self.hw_from_policy(desired_policy) |
| for index, value in FIXED_HARDWARE_TARGETS.items(): |
| desired_hw[index] = value |
| if not np.isfinite(desired_hw).all(): |
| raise RuntimeError("H2 learned target conversion produced non-finite values") |
| return desired_hw.astype(np.float32) |
|
|
| def targets_from_action( |
| self, |
| raw_action_policy: np.ndarray, |
| joint_pos_hw: np.ndarray, |
| joint_vel_hw: np.ndarray, |
| *, |
| joint_margin_rad: float = COMMAND_JOINT_MARGIN_RAD, |
| torque_fraction: float = 1.0, |
| ) -> np.ndarray: |
| """Compatibility helper for a state-sampled constrained action target. |
| |
| This is retained for preflight and non-learned callers. The learned |
| policy loop must use :meth:`learned_raw_hw_targets_from_action` so that |
| its virtual PD target is not position-clipped at 50 Hz. |
| """ |
| action = np.asarray(raw_action_policy, dtype=np.float64).reshape(H2_DOF) |
| q = np.asarray(joint_pos_hw, dtype=np.float64).reshape(H2_DOF) |
| dq = np.asarray(joint_vel_hw, dtype=np.float64).reshape(H2_DOF) |
| if not np.isfinite(action).all() or not np.isfinite(q).all() or not np.isfinite(dq).all(): |
| raise RuntimeError("non-finite values in H2 action/state conversion") |
|
|
| desired_policy = self.default_policy + self.action_scale_policy * action |
| desired_hw = self.hw_from_policy(desired_policy) |
| return self.clip_hw_targets( |
| desired_hw, |
| q, |
| dq, |
| joint_margin_rad=joint_margin_rad, |
| torque_fraction=torque_fraction, |
| fixed_targets=FIXED_HARDWARE_TARGETS, |
| ) |
|
|
| def position_targets_from_action( |
| self, |
| raw_action_policy: np.ndarray, |
| *, |
| joint_margin_rad: float = COMMAND_JOINT_MARGIN_RAD, |
| ) -> np.ndarray: |
| """Compatibility helper that enforces mechanical limits/head lock. |
| |
| Ready/stop code may need position-limited targets, and older tooling |
| imports this method by name. It is intentionally not the learned-policy |
| actuator contract; use :meth:`learned_raw_hw_targets_from_action` there. |
| """ |
|
|
| action = np.asarray(raw_action_policy, dtype=np.float64).reshape(H2_DOF) |
| if not np.isfinite(action).all(): |
| raise RuntimeError("non-finite H2 policy action") |
| desired_policy = self.default_policy + self.action_scale_policy * action |
| desired_hw = self.hw_from_policy(desired_policy) |
| return self.clip_hw_positions( |
| desired_hw, |
| joint_margin_rad=joint_margin_rad, |
| fixed_targets=FIXED_HARDWARE_TARGETS, |
| ) |
|
|
| def clip_hw_positions( |
| self, |
| desired_hw: np.ndarray, |
| *, |
| joint_margin_rad: float = COMMAND_JOINT_MARGIN_RAD, |
| fixed_targets: dict[int, float] | None = None, |
| ) -> np.ndarray: |
| """Apply only the invariant mechanical position interval.""" |
|
|
| desired = np.asarray(desired_hw, dtype=np.float64).reshape(H2_DOF) |
| if not np.isfinite(desired).all(): |
| raise RuntimeError("non-finite values in H2 position clipping") |
| lo, hi = command_position_bounds_hw( |
| self.joint_lo_hw, |
| self.joint_hi_hw, |
| joint_margin_rad=joint_margin_rad, |
| ) |
| result = np.clip(desired, lo, hi) |
| for index, value in (fixed_targets or {}).items(): |
| fixed = float(value) |
| if not np.isfinite(fixed) or not lo[index] <= fixed <= hi[index]: |
| raise RuntimeError( |
| f"fixed H2 target for {HARDWARE_JOINT_NAMES[index]} is outside position limits" |
| ) |
| result[index] = fixed |
| return result.astype(np.float32) |
|
|
| def clip_hw_targets( |
| self, |
| desired_hw: np.ndarray, |
| joint_pos_hw: np.ndarray, |
| joint_vel_hw: np.ndarray, |
| *, |
| joint_margin_rad: float = COMMAND_JOINT_MARGIN_RAD, |
| torque_fraction: float = 1.0, |
| fixed_targets: dict[int, float] | None = None, |
| ) -> np.ndarray: |
| """Intersect H2 position and PD-torque limits for every target. |
| |
| Applying two independent ``clip`` operations is unsafe: the second can |
| push a target back outside the first interval. This method instead |
| computes the actual feasible intersection and rejects an empty one. |
| Exact fixed targets (the policy head lock) must themselves lie inside |
| that intersection or the caller fails closed. |
| """ |
| desired_hw = np.asarray(desired_hw, dtype=np.float64).reshape(H2_DOF) |
| q = np.asarray(joint_pos_hw, dtype=np.float64).reshape(H2_DOF) |
| dq = np.asarray(joint_vel_hw, dtype=np.float64).reshape(H2_DOF) |
| if not np.isfinite(desired_hw).all() or not np.isfinite(q).all() or not np.isfinite(dq).all(): |
| raise RuntimeError("non-finite values in H2 target clipping") |
|
|
| lo_pos32, hi_pos32 = command_position_bounds_hw( |
| self.joint_lo_hw, |
| self.joint_hi_hw, |
| joint_margin_rad=joint_margin_rad, |
| ) |
| fraction = float(torque_fraction) |
| if not np.isfinite(fraction) or not 0.0 < fraction <= 1.0: |
| raise ValueError("torque_fraction must be finite and in (0,1]") |
| lo_pos = lo_pos32.astype(np.float64) |
| hi_pos = hi_pos32.astype(np.float64) |
| |
| torque_budget = fraction * self.torque_hw |
| target_lo = q + (-torque_budget + self.kd_hw * dq) / self.kp_hw |
| target_hi = q + (torque_budget + self.kd_hw * dq) / self.kp_hw |
| torque_lo = np.minimum(target_lo, target_hi) |
| torque_hi = np.maximum(target_lo, target_hi) |
| feasible_lo = np.maximum(lo_pos, torque_lo) |
| feasible_hi = np.minimum(hi_pos, torque_hi) |
| empty = np.flatnonzero(feasible_lo > feasible_hi) |
| if empty.size: |
| index = int(empty[0]) |
| raise RuntimeError( |
| "no position/torque-feasible H2 target for " |
| f"{HARDWARE_JOINT_NAMES[index]}: position=[{lo_pos[index]:.4f},{hi_pos[index]:.4f}] " |
| f"torque=[{torque_lo[index]:.4f},{torque_hi[index]:.4f}]" |
| ) |
|
|
| result = np.clip(desired_hw, feasible_lo, feasible_hi) |
| for raw_index, raw_value in (fixed_targets or {}).items(): |
| index = int(raw_index) |
| value = float(raw_value) |
| if not 0 <= index < H2_DOF or not np.isfinite(value): |
| raise RuntimeError(f"invalid fixed H2 target {raw_index!r}={raw_value!r}") |
| if value < feasible_lo[index] or value > feasible_hi[index]: |
| raise RuntimeError( |
| f"fixed H2 target {HARDWARE_JOINT_NAMES[index]}={value:.4f} is not " |
| f"position/torque feasible [{feasible_lo[index]:.4f},{feasible_hi[index]:.4f}]" |
| ) |
| result[index] = value |
| if not np.isfinite(result).all(): |
| raise RuntimeError("H2 target clipping produced non-finite values") |
| if np.any(result < lo_pos - 1.0e-7) or np.any(result > hi_pos + 1.0e-7): |
| raise RuntimeError("H2 target clipping escaped the joint-position interval") |
| if np.any(result < torque_lo - 1.0e-7) or np.any(result > torque_hi + 1.0e-7): |
| raise RuntimeError("H2 target clipping escaped the PD-torque interval") |
| for index, value in (fixed_targets or {}).items(): |
| if abs(float(result[int(index)]) - float(value)) > 1.0e-7: |
| raise RuntimeError("H2 fixed-target invariant failed") |
| return result.astype(np.float32) |
|
|
| def first_limit_violation(self, q_hw: np.ndarray, tolerance_rad: float) -> tuple[int, float] | None: |
| q = np.asarray(q_hw, dtype=np.float64).reshape(H2_DOF) |
| if not np.isfinite(q).all(): |
| raise RuntimeError("H2 joint state contains non-finite values") |
| tol = max(0.0, float(tolerance_rad)) |
| bad = np.flatnonzero((q < self.joint_lo_hw - tol) | (q > self.joint_hi_hw + tol)) |
| if bad.size == 0: |
| return None |
| index = int(bad[0]) |
| return index, float(q[index]) |
|
|
|
|
| __all__ = [ |
| "COMMAND_JOINT_MARGIN_RAD", |
| "command_position_bounds_hw", |
| "EXPECTED_HW_TO_POLICY", |
| "EXPECTED_POLICY_TO_HW", |
| "EXPECTED_POLICY_JOINT_NAMES", |
| "FIXED_HARDWARE_TARGETS", |
| "H2_JOINT_MAP_REVISION", |
| "H2Contract", |
| "H2_DOF", |
| "HARDWARE_JOINT_NAMES", |
| "LOWCMD_DOF", |
| "MODE_PR", |
| ] |
|
|
