File size: 22,280 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
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
"""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
# Must remain identical to C++ ``kJointMargin``.  A smaller Python margin can
# produce a command that Python accepts but the 1000 Hz bridge rejects.
COMMAND_JOINT_MARGIN_RAD = 0.01

# Unitree's H2 examples currently disagree about the waist/wrist slot order.
# This deployment is pinned to the C++ unitree_sdk2 table installed beside the
# connected robot.  Offline A/B replay of the 2026-09-04 real L1 snapshot also
# reproduces the observed collapse when the later Python-only table is used,
# while this C++ order stays stable.  Changing this order is a semantic ABI
# change and requires a new H2BR version plus a fresh physical ID check.
H2_JOINT_MAP_REVISION = "unitree_sdk2@e02dc701-h2-cpp"

# H2 low-level order from unitree_sdk2 commit
# e02dc701548fc81a5fa06e6734e98540329a2377.  In PR mode, ankle slots 4/5 and
# 10/11 mean roll/pitch.  In AB mode they do not, which is why the bridge
# rejects any mode other than MODE_PR.
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",
)

# Exact joint order embedded in the supplied H2 ONNX export.
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",
)

# Checked constants are retained as an independent tripwire.  Runtime mapping
# is still constructed from names below.
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]  # type: ignore[arg-type]
    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))

    # Fall back to actuator ctrlrange only if a joint-level force range is absent.
    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)
        # tau = kp * (target-q) - kd*dq.  Solve |tau| <= torque_limit for target.
        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",
]