feat(open_door): enhance task configuration for door operation

aao_configs/open_door.yaml

@@ -6,14 +6,31 @@ scene_name: open_door
 task_name: open_door
 
 env:
-  # Task-specific arm home — gripper facing the door handle. Shifted for the
-  # consolidated demo.xml: door frame is at world (1.54, 0.1, -1.0) vs the
-  # legacy robotiq scene's (1.49, 0.1, 0.0), so the arm home moves +0.05 x
-  # and -1.0 z to keep the same relative pose to the handle.
+  # Task-specific arm home — gripper horizontal, pointing +X (fingers toward the
+  # door, body/wrist toward the robot). The robotiq palm/wrist sits ~0.14m BEHIND
+  # the fingertips, so pointing +X keeps the bulky body clear of both the lever
+  # and the door panel (only ~8cm apart) while the fingers reach in to clamp the
+  # lever bar vertically (jaw along world Z, straddling the bar top/bottom).
+  # NOTE: for the mocap gripper this value is the *mocap target*; the TCP/pads
+  # land ~0.34m ahead of it, so this freejoint pose was calibrated so the open
+  # finger pads sit ~5.5cm in front of the bar (a clean pre-grasp) with the palm
+  # back at x~1.27 (clear of the 1.46 bar and 1.52 panel).
   initial_joint_positions:
-    robotiq_freejoint: [1.285, 0.3155, -0.195, 0.5, 0.5, -0.5, -0.5]
+    robotiq_freejoint: [1.057, 0.436, -0.367, -0.5, -0.5, 0.5, 0.5]
   mask_objects: ["handle_lever_body", "handle_body_phys"]
-  operations: ["pick", "push"]
+  operations: ["push"]
+  # Door latch: keeps door_hinge spring-locked near zero until the handle is
+  # pressed past unlock_threshold. Without this the door swings freely the
+  # instant the gripper touches it, so the handle never gets pressed and the
+  # approach just bulldozes the (unlatched) door.
+  pre_step_callbacks:
+    - _target_: auto_atom.callbacks.door_latch.DoorLatchCallback
+      door_joint: door_hinge
+      handle_joint: handle_hinge
+      kp: 80.0
+      kd: 8.0
+      unlock_threshold: 0.12
+      lock_zone: 0.05
   viewer:
     lookat: [1.45, 0.12, 0.8]
     distance: 1.2
@@ -23,59 +40,95 @@ env:
     hold_seconds: 1.0
 
 task:
+  randomization:
+    arm:
+      eef:
+        x: [-0.03, 0.03]
+        y: [-0.03, 0.03]
+        z: [-0.03, 0.03]
+        roll: [-0.0873, 0.0873]
+        pitch: [-0.0873, 0.0873]
+        yaw: [-0.0873, 0.0873]
   stages:
-    - name: rotate_handle
-      object: handle_lever_body
-      operation: pick
+    # One continuous push: press the lever down to unlock, then swing the door
+    # open. The success object is handle_body_phys (rides on the door panel), so
+    # the door's translation satisfies the "pushed" displacement check — the
+    # lever only rotates in place, which a displacement check would reject.
+    - name: open_door
+      object: handle_body_phys
+      operation: push
       operator: arm
       param:
         pre_move:
-          - position: [0.095, 0.106, 0.0]
-            reference: object
-            max_linear_step: 0.02
+          # 0. Re-home in front of the lever bar with the open finger pads
+          #    straddling it in Z (~5.5cm in front). The mocap reset settles to a
+          #    slightly different pose per batch env, so drive to an explicit
+          #    object-referenced point and a fixed world orientation to converge
+          #    every env. Without the explicit orientation, reset randomization is
+          #    inherited by the push arc: some envs press the lever but fail to
+          #    maintain the contact geometry needed to move the door.
+          - position: [-0.13, 0.079, 0.0]
+            orientation: [0.71913563, 0.00524467, 0.05427597, 0.69272690]
+            reference: object_world
+            max_linear_step: 0.2
+            max_angular_step: 0.3
+            # The global 15 mm tolerance is wider than the 12 mm handle.
+            # Converging here prevents reset variation from shifting the grasp.
+            tolerance:
+              position: 0.003
+              orientation: 0.02
+          # 1. Advance +X so the bar enters the jaw (between the top/bottom pads).
+          - position: [0.06, 0.0, 0.0]
+            reference: eef_world
+            max_linear_step: 0.2
             max_angular_step: 0.25
-          - position: [0.095, 0.055, 0.0]
-            reference: object
-            max_linear_step: 0.01
-            max_angular_step: 0.2
+            # This value controls insertion depth: both 2 mm and 3.5 mm were
+            # substantially less reliable in the randomized batch test.
+            tolerance:
+              position: 0.003
+              orientation: 0.02
         eef:
           close: true
         post_move:
+          # 2. Press the lever down to unlock. handle_hinge's local Y axis maps
+          #    to world -X (door frame is rotated +90deg about Z), so the arc
+          #    axis is world [-1,0,0]; a positive target angle presses the lever
+          #    down past the latch's unlock_threshold. max_step 0.15 keeps each
+          #    arc sub-step (>~0.014m) above the 0.01m tolerance so it moves.
           - arc:
               pivot: handle_hinge
-              axis: [0, 1, 0]
+              axis: [-1, 0, 0]
               angle: 0.45
               absolute: true
-              max_step: 0.03
-            reference: world
-    - name: push_open
-      object: handle_body_phys
-      operation: push
-      operator: arm
-      param:
-        pre_move:
-          - position: [0.0, 0.0, 0.0]
-            reference: eef_world
-            max_linear_step: 0.01
-            max_angular_step: 0.15
-        post_move:
-          - arc:
-              pivot: handle_hinge
-              axis: [0, 1, 0]
-              angle: 0.55
-              absolute: true
-              max_step: 0.03
+              max_step: 0.15
             reference: world
+          # 3. Swing the door open by sweeping the gripper +0.2rad around the
+          #    door_hinge (world +Z). RELATIVE (not absolute) on purpose: a
+          #    snapshot-based arc rotates the gripper from its pose at action
+          #    start by the cumulative angle, so the orientation target stays a
+          #    clean horizontal Z-sweep. An absolute arc instead re-references
+          #    the live (contact-deflected) pose every tick, so when the door
+          #    momentarily stalls on its high frictionloss (5.0) the grip twists
+          #    the gripper and it accumulates into the wrist flipping to point
+          #    straight up. The per-waypoint tolerance lets the sweep finish when
+          #    the door stalls a hair short of the full angle.
           - arc:
               pivot: door_hinge
-              axis: [0, 0, -1]
-              angle: -1.2
-              absolute: true
-              max_step: 0.04
+              axis: [0, 0, 1]
+              angle: 0.2
+              max_step: 0.05
             reference: world
+            tolerance:
+              position: 0.10
+              orientation: 0.12
 
-operators:
-  - name: arm
+task_operators:
+  arm:
     control:
-      cartesian_max_linear_step: 0.015
-      cartesian_max_angular_step: 0.2
+      cartesian_max_linear_step: 0.025
+      cartesian_max_angular_step: 0.3
+      timeout_steps: 300
+      tolerance:
+        position: 0.015
+        orientation: 0.12
+        eef: 0.012