From 10e420d2cf0181f23d448ddf93a81f8485b4e124 Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Tue, 17 Feb 2026 22:10:05 -0600 Subject: [PATCH] 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 --- teleop/teleop_hand_and_arm.py | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 0b58818..e630ba8 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/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: