Make A/r toggle tracking on/off, allow arm reset while paused

- A button and r key now toggle tracking (start/pause) instead of one-way start
- X/h arm reset works regardless of tracking state (can pause then reset)
- I-term accumulators cleared when paused to prevent stale drift correction
- Main loop stays alive while paused (buttons still polled, can resume with A)

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>

teleop/teleop_hand_and_arm.py

@@ -72,7 +72,8 @@ _r3_prev_buttons = {}
 def on_press(key):
     global STOP, START, RECORD_TOGGLE, INTEGRAL_ENABLED, RESET_ARMS
     if key == 'r':
-        START = True
+        START = not START
+        logger_mp.info(f"[key] r pressed → tracking {'STARTED' if START else 'PAUSED'}")
     elif key == 'q':
         START = False
         STOP = True
@@ -81,7 +82,7 @@ def on_press(key):
     elif key == 'i':
         INTEGRAL_ENABLED = not INTEGRAL_ENABLED
         logger_mp.info(f"[on_press] Integral correction {'ENABLED' if INTEGRAL_ENABLED else 'DISABLED'}")
-    elif key == 'h' and START == True:
+    elif key == 'h':
         RESET_ARMS = True
         logger_mp.info("[on_press] Arm reset requested (return to home)")
     else:
@@ -273,7 +274,7 @@ if __name__ == '__main__':
         logger_mp.info("----------------------------------------------------------------")
         logger_mp.info("  Keyboard  |  R3 Controller  |  Action")
         logger_mp.info("  ---------   ---------------    ------")
-        logger_mp.info("    [r]     |     [A]         |  Start arm tracking")
+        logger_mp.info("    [r]     |     [A]         |  Toggle arm tracking")
         logger_mp.info("    [q]     |     [B]         |  Stop and exit")
         logger_mp.info("    [h]     |     [X]         |  Reset arms to home")
         logger_mp.info("    [i]     |     [Y]         |  Toggle I-term correction")
@@ -331,14 +332,13 @@ if __name__ == '__main__':
             r3_data = arm_ctrl.get_wireless_remote()
             r3_btns = parse_r3_buttons(r3_data)
             if r3_btns.get('A') and not _r3_prev_buttons.get('A'):
-                if not START:
-                    START = True
-                    logger_mp.info("[R3] A pressed → START tracking")
+                START = not START
+                logger_mp.info(f"[R3] A pressed → tracking {'STARTED' if START else 'PAUSED'}")
             if r3_btns.get('B') and not _r3_prev_buttons.get('B'):
                 START = False
                 STOP = True
                 logger_mp.info("[R3] B pressed → STOP")
-            if r3_btns.get('X') and not _r3_prev_buttons.get('X') and START:
+            if r3_btns.get('X') and not _r3_prev_buttons.get('X'):
                 RESET_ARMS = True
                 logger_mp.info("[R3] X pressed → Reset arms")
             if r3_btns.get('Y') and not _r3_prev_buttons.get('Y'):
@@ -349,6 +349,16 @@ if __name__ == '__main__':
                 logger_mp.info("[R3] Start pressed → Toggle recording")
             _r3_prev_buttons = r3_btns
 
+            # Skip arm tracking while paused (buttons + reset still work)
+            if not START:
+                i_error_left = np.zeros(3)
+                i_error_right = np.zeros(3)
+                current_time = time.time()
+                time_elapsed = current_time - start_time
+                sleep_time = max(0, (1 / args.frequency) - time_elapsed)
+                time.sleep(sleep_time)
+                continue
+
             # get image
             if camera_config['head_camera']['enable_zmq']:
                 if args.record or xr_need_local_img: