fix(isaac): make kinematic-tree guard fail-first, fix G1 topology

cagataycali's comment at strands_robots_sim/isaac/tests/test_procedural_kinematic_guard.py:13
on PR #51 pushed back on the opt-in env-var guard from def3c21:
shipping a robot we know cannot instantiate has no good use case
in this package, and the default should fail first rather than
silently producing a broken robot.

This commit lands the fix-first answer rather than a flag-flip:

* _validate_kinematic_tree now runs unconditionally on every
  procedural builder + every URDF/MJCF/USD loader. The
  STRANDS_ISAAC_VALIDATE_KINEMATICS env-var escape hatch is
  removed; there is no use case for opting out of a check that
  only fires on robots that cannot instantiate.

* _build_unitree_g1 inserts six massless intermediate '*_link'
  bodies (l_hip_link, r_hip_link, l_ankle_link, r_ankle_link,
  l_elbow_link, r_elbow_link) so each 2-DOF compound joint
  (hips, ankles, shoulder-yaw/elbow on each arm) splits across
  two unique (parent, child) edges. Actuated joint count stays
  at 21; only the topology gains the six fixed link bodies, so
  the existing 21-DOF doc-pin in test_procedural_g1_dof.py keeps
  passing.

* test_procedural_kinematic_guard.py is rewritten to pin the
  fail-first contract:
  - all three shipped robots build cleanly under the default
    guard (test_g1_builds_cleanly_by_default,
    test_so100_and_panda_build_cleanly)
  - G1 topology has zero duplicate edges
    (test_g1_topology_has_no_duplicate_edges)
  - an injected duplicate edge raises with diagnostic context
    (test_guard_raises_on_injected_duplicate_edge)
  - procedural.py source contains no env-var gate or os.environ
    read for the guard (test_guard_has_no_env_var_escape_hatch)
    -- a future refactor that re-introduces the escape hatch
    will fail this pin.

hatch run test: 44 passed, 2 skipped.
hatch run lint: clean.

Author: yinsong1986

strands_robots_sim/isaac/procedural.py

@@ -11,7 +11,6 @@
 
 from __future__ import annotations
 
-import os
 from dataclasses import dataclass, field
 
 
@@ -64,29 +63,34 @@ def joint_names(self) -> list[str]:
 
 
 def _validate_kinematic_tree(robot: ProceduralRobot) -> None:
-    """Phase-1 fail-fast guard: reject duplicate (parent, child) body edges.
-
-    Phase-1 callers don't instantiate the articulation, so this is a no-op at
-    builder time UNLESS ``STRANDS_ISAAC_VALIDATE_KINEMATICS=1`` is set
-    (Phase-2 dev path). When the guard fires, it surfaces the defect with
-    body indices + joint names rather than letting USD/MuJoCo emit a cryptic
-    articulation error two layers down.
-
-    See the NOTE in ``_build_unitree_g1`` for the specific defect this is
-    designed to catch (G1 legs/arms currently share a single ``(parent,
-    child)`` edge across two joint axes; USD requires intermediate massless
-    link bodies).
+    """Fail-fast guard: reject duplicate (parent, child) body edges.
+
+    A USD/MuJoCo articulation requires a tree where each non-root link has
+    exactly one inbound joint. Two joints sharing the same ``(parent_body,
+    child_body)`` edge violate that invariant and would surface two layers
+    down as a cryptic articulation error at instantiation time.
+
+    Validation runs unconditionally on every procedural builder + every
+    URDF / MJCF / USD loader: shipping a robot we know cannot instantiate
+    has no good use case in this package, and silently producing one is
+    worse than failing fast at builder time. The check raises ``ValueError``
+    with body indices + joint names so the offender is obvious from the
+    traceback alone.
+
+    For 2-DOF compound joints (e.g. a hip with both roll and pitch axes),
+    insert an intermediate massless link body between the two joints so each
+    joint has its own ``(parent, child)`` edge. ``_build_unitree_g1`` is the
+    canonical example -- six ``*_link`` intermediate bodies split the
+    hip / ankle / arm 2-DOF axes.
     """
-    if os.environ.get("STRANDS_ISAAC_VALIDATE_KINEMATICS", "").lower() not in ("1", "true", "yes"):
-        return
     from collections import Counter
 
     edges = [(j.parent_body, j.child_body) for j in robot.joints]
     dups = {edge: count for edge, count in Counter(edges).items() if count > 1}
     if dups:
         offenders = {edge: [j.name for j in robot.joints if (j.parent_body, j.child_body) == edge] for edge in dups}
         raise ValueError(
-            f"{robot.name}: duplicate parent->child body edges (Phase-2 defect): {offenders}. "
+            f"{robot.name}: duplicate parent->child body edges: {offenders}. "
             f"Insert intermediate massless link bodies before instantiating articulation."
         )
 
@@ -170,7 +174,15 @@ def _build_panda() -> ProceduralRobot:
 
 
 def _build_unitree_g1() -> ProceduralRobot:
-    """Build Unitree G1 humanoid (simplified 21-DOF) procedurally."""
+    """Build Unitree G1 humanoid (simplified 21-DOF) procedurally.
+
+    The G1 has six 2-DOF compound joints (hips, ankles, shoulder/elbow on each
+    arm). To keep the kinematic graph a valid tree -- one inbound joint per
+    non-root link, as USD / MuJoCo articulations require -- each compound
+    joint is split through a massless intermediate ``*_link`` body. The two
+    axes still resolve to two actuated joints (so ``num_joints == 21``); only
+    the topology gains the six extra fixed link bodies.
+    """
     bodies = [
         BodyDef(name="pelvis", position=(0.0, 0.0, 0.85), mass=10.0, shape="box", shape_size=(0.15, 0.1, 0.15)),
         BodyDef(name="torso", position=(0.0, 0.0, 1.1), mass=8.0, shape="box", shape_size=(0.12, 0.08, 0.3)),
@@ -193,59 +205,60 @@ def _build_unitree_g1() -> ProceduralRobot:
         BodyDef(name="r_shoulder", position=(0.2, 0.0, 1.2), mass=1.5, shape="sphere", shape_size=(0.04,)),
         BodyDef(name="r_upper_arm", position=(0.35, 0.0, 1.2), mass=1.5, shape="capsule", shape_size=(0.03, 0.12)),
         BodyDef(name="r_forearm", position=(0.55, 0.0, 1.2), mass=1.0, shape="capsule", shape_size=(0.025, 0.1)),
+        # Massless intermediate link bodies so 2-DOF compound joints (hips,
+        # ankles, shoulder-yaw/elbow) each have a unique (parent, child) edge.
+        # Indices 17..22.
+        BodyDef(name="l_hip_link", position=(-0.08, 0.0, 0.7), mass=0.0, shape="sphere", shape_size=(0.001,)),
+        BodyDef(name="r_hip_link", position=(0.08, 0.0, 0.7), mass=0.0, shape="sphere", shape_size=(0.001,)),
+        BodyDef(name="l_ankle_link", position=(-0.08, 0.0, 0.1), mass=0.0, shape="sphere", shape_size=(0.001,)),
+        BodyDef(name="r_ankle_link", position=(0.08, 0.0, 0.1), mass=0.0, shape="sphere", shape_size=(0.001,)),
+        BodyDef(name="l_elbow_link", position=(-0.45, 0.0, 1.2), mass=0.0, shape="sphere", shape_size=(0.001,)),
+        BodyDef(name="r_elbow_link", position=(0.45, 0.0, 1.2), mass=0.0, shape="sphere", shape_size=(0.001,)),
     ]
 
-    # Simplified joint set (21 DOF total: 1 torso + 6 left leg + 6 right leg + 4 left arm + 4 right arm).
-    # NOTE: this kinematic graph contains duplicate (parent, child) edges on each leg/arm
-    # (e.g. l_hip_roll and l_hip_pitch both map bodies 3 -> 4). A real USD/MuJoCo articulation
-    # builder requires a tree where each non-root link has exactly one inbound joint, so this
-    # topology will need intermediate massless link bodies before Phase 2 wires up the actual
-    # USD prim chain. Tracked as Phase-2 work; the Phase-1 skeleton does not instantiate the
-    # articulation, so the duplicate-edge defect is dormant on this branch.
-    #
-    # ``_validate_kinematic_tree`` (called below) is a no-op by default but raises when
-    # ``STRANDS_ISAAC_VALIDATE_KINEMATICS=1`` is set, which is the Phase-2 dev path: it
-    # surfaces this defect at builder time with body indices + joint names rather than
-    # letting USD/MuJoCo emit a cryptic articulation error two layers down.
+    # 21 DOF total: 1 torso + 6 left leg + 6 right leg + 4 left arm + 4 right arm.
+    # Each 2-DOF compound joint is split through a massless intermediate body
+    # (indices 17..22) so every (parent, child) edge in the kinematic tree is
+    # unique -- the invariant ``_validate_kinematic_tree`` enforces below.
     joints = [
         # Torso
         JointDef(name="torso_yaw", parent_body=0, child_body=1, axis=(0, 0, 1), limit_lower=-1.0, limit_upper=1.0),
-        # Left leg (6 DOF)
+        # Left leg (6 DOF). Hip 2-DOF split via l_hip_link (17); ankle 2-DOF split via l_ankle_link (19).
         JointDef(name="l_hip_yaw", parent_body=0, child_body=3, axis=(0, 0, 1), limit_lower=-0.5, limit_upper=0.5),
-        JointDef(name="l_hip_roll", parent_body=3, child_body=4, axis=(1, 0, 0), limit_lower=-0.5, limit_upper=0.5),
-        JointDef(name="l_hip_pitch", parent_body=3, child_body=4, axis=(0, 1, 0), limit_lower=-1.5, limit_upper=0.5),
+        JointDef(name="l_hip_roll", parent_body=3, child_body=17, axis=(1, 0, 0), limit_lower=-0.5, limit_upper=0.5),
+        JointDef(name="l_hip_pitch", parent_body=17, child_body=4, axis=(0, 1, 0), limit_lower=-1.5, limit_upper=0.5),
         JointDef(name="l_knee", parent_body=4, child_body=5, axis=(0, 1, 0), limit_lower=-0.1, limit_upper=2.5),
-        JointDef(name="l_ankle_pitch", parent_body=5, child_body=6, axis=(0, 1, 0), limit_lower=-0.8, limit_upper=0.5),
-        JointDef(name="l_ankle_roll", parent_body=5, child_body=6, axis=(1, 0, 0), limit_lower=-0.3, limit_upper=0.3),
-        # Right leg (6 DOF)
+        JointDef(name="l_ankle_pitch", parent_body=5, child_body=19, axis=(0, 1, 0), limit_lower=-0.8, limit_upper=0.5),
+        JointDef(name="l_ankle_roll", parent_body=19, child_body=6, axis=(1, 0, 0), limit_lower=-0.3, limit_upper=0.3),
+        # Right leg (6 DOF). Hip 2-DOF split via r_hip_link (18); ankle 2-DOF split via r_ankle_link (20).
         JointDef(name="r_hip_yaw", parent_body=0, child_body=7, axis=(0, 0, 1), limit_lower=-0.5, limit_upper=0.5),
-        JointDef(name="r_hip_roll", parent_body=7, child_body=8, axis=(1, 0, 0), limit_lower=-0.5, limit_upper=0.5),
-        JointDef(name="r_hip_pitch", parent_body=7, child_body=8, axis=(0, 1, 0), limit_lower=-1.5, limit_upper=0.5),
+        JointDef(name="r_hip_roll", parent_body=7, child_body=18, axis=(1, 0, 0), limit_lower=-0.5, limit_upper=0.5),
+        JointDef(name="r_hip_pitch", parent_body=18, child_body=8, axis=(0, 1, 0), limit_lower=-1.5, limit_upper=0.5),
         JointDef(name="r_knee", parent_body=8, child_body=9, axis=(0, 1, 0), limit_lower=-0.1, limit_upper=2.5),
-        JointDef(name="r_ankle_pitch", parent_body=9, child_body=10, axis=(0, 1, 0), limit_lower=-0.8, limit_upper=0.5),
-        JointDef(name="r_ankle_roll", parent_body=9, child_body=10, axis=(1, 0, 0), limit_lower=-0.3, limit_upper=0.3),
-        # Left arm (4 DOF simplified)
+        JointDef(name="r_ankle_pitch", parent_body=9, child_body=20, axis=(0, 1, 0), limit_lower=-0.8, limit_upper=0.5),
+        JointDef(name="r_ankle_roll", parent_body=20, child_body=10, axis=(1, 0, 0), limit_lower=-0.3, limit_upper=0.3),
+        # Left arm (4 DOF simplified). Shoulder-yaw / elbow 2-DOF split via l_elbow_link (21).
         JointDef(
             name="l_shoulder_pitch", parent_body=1, child_body=11, axis=(0, 1, 0), limit_lower=-3.14, limit_upper=1.0
         ),
         JointDef(
             name="l_shoulder_roll", parent_body=11, child_body=12, axis=(1, 0, 0), limit_lower=-0.3, limit_upper=3.14
         ),
         JointDef(
-            name="l_shoulder_yaw", parent_body=12, child_body=13, axis=(0, 0, 1), limit_lower=-1.5, limit_upper=1.5
+            name="l_shoulder_yaw", parent_body=12, child_body=21, axis=(0, 0, 1), limit_lower=-1.5, limit_upper=1.5
         ),
-        JointDef(name="l_elbow", parent_body=12, child_body=13, axis=(0, 1, 0), limit_lower=-2.5, limit_upper=0.0),
-        # Right arm (4 DOF simplified)
+        JointDef(name="l_elbow", parent_body=21, child_body=13, axis=(0, 1, 0), limit_lower=-2.5, limit_upper=0.0),
+        # Right arm (4 DOF simplified). Shoulder-yaw / elbow 2-DOF split via r_elbow_link (22).
         JointDef(
             name="r_shoulder_pitch", parent_body=1, child_body=14, axis=(0, 1, 0), limit_lower=-3.14, limit_upper=1.0
         ),
         JointDef(
             name="r_shoulder_roll", parent_body=14, child_body=15, axis=(1, 0, 0), limit_lower=-3.14, limit_upper=0.3
         ),
         JointDef(
-            name="r_shoulder_yaw", parent_body=15, child_body=16, axis=(0, 0, 1), limit_lower=-1.5, limit_upper=1.5
+            name="r_shoulder_yaw", parent_body=15, child_body=22, axis=(0, 0, 1), limit_lower=-1.5, limit_upper=1.5
         ),
-        JointDef(name="r_elbow", parent_body=15, child_body=16, axis=(0, 1, 0), limit_lower=-2.5, limit_upper=0.0),
+        JointDef(name="r_elbow", parent_body=22, child_body=16, axis=(0, 1, 0), limit_lower=-2.5, limit_upper=0.0),
     ]
 
     robot = ProceduralRobot(name="unitree_g1", bodies=bodies, joints=joints, base_position=(0.0, 0.0, 0.85))

strands_robots_sim/isaac/tests/test_procedural_g1_dof.py

@@ -11,10 +11,9 @@
 ``test_phase1_doc_banner.py`` (lands in PR-5 alongside the docs file
 itself).
 
-Note: this pin does NOT cover the kinematic-tree topology defect on the
-G1 joint graph (duplicate ``(parent_body, child_body)`` edges); that is
-a Phase 2 fix that needs intermediate massless link bodies, and is
-documented in ``procedural.py`` line 165 as deferred.
+Note: this pin does NOT cover the kinematic-tree topology validity (each
+non-root link has exactly one inbound joint); that invariant is pinned
+separately in ``test_procedural_kinematic_guard.py``.
 """
 
 from __future__ import annotations