From 1a3c2d77d9c97be14548c131061dd1ad69a74de7 Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Tue, 17 Feb 2026 21:44:42 -0600 Subject: [PATCH] Add integral drift correction, arm reset key, and I-term toggle Adds a closed-loop integral (I) term to compensate for arm drift caused by the 250Hz velocity clipper. When the user moves faster than the clip limit, the robot falls behind; the I-term accumulates the task-space position error (FK vs VP target) and biases future IK targets to catch up. New features: - --ki/--i-clamp/--i-decay CLI args for tunable gain, anti-windup, decay - 'h' key resets arms to home position without exiting the program - 'i' key toggles integral correction on/off mid-session - Per-second logging of error magnitude, I accumulator, and correction - compute_fk() method on G1_29_ArmIK for Pinocchio forward kinematics Co-Authored-By: Claude Opus 4.6 --- teleop/robot_control/robot_arm_ik.py | 19 +++++- teleop/teleop_hand_and_arm.py | 87 +++++++++++++++++++++++++++- 2 files changed, 102 insertions(+), 4 deletions(-) diff --git a/teleop/robot_control/robot_arm_ik.py b/teleop/robot_control/robot_arm_ik.py index c5665d6..c7645d2 100644 --- a/teleop/robot_control/robot_arm_ik.py +++ b/teleop/robot_control/robot_arm_ik.py @@ -308,7 +308,24 @@ class G1_29_ArmIK: # return sol_q, sol_tauff return current_lr_arm_motor_q, np.zeros(self.reduced_robot.model.nv) - + + def compute_fk(self, current_lr_arm_q): + """Compute forward kinematics for both end-effectors. + + Args: + current_lr_arm_q: 14-dim numpy array of current joint angles + + Returns: + left_ee_pos: 3-dim numpy array, position of L_ee in waist frame + right_ee_pos: 3-dim numpy array, position of R_ee in waist frame + """ + q = np.array(current_lr_arm_q) + pin.forwardKinematics(self.reduced_robot.model, self.reduced_robot.data, q) + pin.updateFramePlacements(self.reduced_robot.model, self.reduced_robot.data) + left_ee_pos = self.reduced_robot.data.oMf[self.L_hand_id].translation.copy() + right_ee_pos = self.reduced_robot.data.oMf[self.R_hand_id].translation.copy() + return left_ee_pos, right_ee_pos + class G1_23_ArmIK: def __init__(self, Unit_Test = False, Visualization = False): np.set_printoptions(precision=5, suppress=True, linewidth=200) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 8cafba3..9b9a5b6 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -1,5 +1,6 @@ import time import argparse +import numpy as np from multiprocessing import Value, Array, Lock import threading import logging_mp @@ -36,6 +37,8 @@ STOP = False # Enable to begin system exit procedure READY = False # Ready to (1) enter START state, (2) enter RECORD_RUNNING state RECORD_RUNNING = False # True if [Recording] RECORD_TOGGLE = False # Toggle recording state +INTEGRAL_ENABLED = True # Toggle integral drift correction via 'i' key +RESET_ARMS = False # Trigger arm reset via 'h' key # ------- --------- ----------- ----------- --------- # state [Ready] ==> [Recording] ==> [AutoSave] --> [Ready] # ------- --------- | ----------- | ----------- | --------- @@ -49,7 +52,7 @@ RECORD_TOGGLE = False # Toggle recording state # --> auto : Auto-transition after saving data. def on_press(key): - global STOP, START, RECORD_TOGGLE + global STOP, START, RECORD_TOGGLE, INTEGRAL_ENABLED, RESET_ARMS if key == 'r': START = True elif key == 'q': @@ -57,6 +60,12 @@ def on_press(key): STOP = True elif key == 's' and START == True: RECORD_TOGGLE = True + 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: + RESET_ARMS = True + logger_mp.info("[on_press] Arm reset requested (return to home)") else: logger_mp.warning(f"[on_press] {key} was pressed, but no action is defined for this key.") @@ -86,6 +95,10 @@ if __name__ == '__main__': parser.add_argument('--sim', action = 'store_true', help = 'Enable isaac simulation mode') parser.add_argument('--ipc', action = 'store_true', help = 'Enable IPC server to handle input; otherwise enable sshkeyboard') parser.add_argument('--affinity', action = 'store_true', help = 'Enable high priority and set CPU affinity mode') + # integral drift correction + parser.add_argument('--ki', type=float, default=0.3, help='Integral gain for drift correction (0.0 = disabled)') + parser.add_argument('--i-clamp', type=float, default=0.10, help='Anti-windup clamp for I term in meters (default: 0.10)') + parser.add_argument('--i-decay', type=float, default=0.995, help='Per-frame decay factor for I term (default: 0.995)') # record mode and task info parser.add_argument('--record', action = 'store_true', help = 'Enable data recording mode') parser.add_argument('--task-dir', type = str, default = './utils/data/', help = 'path to save data') @@ -245,6 +258,11 @@ if __name__ == '__main__': logger_mp.info("🟡 Press [s] to START or SAVE recording (toggle cycle).") else: logger_mp.info("🔵 Recording is DISABLED (run with --record to enable).") + logger_mp.info("🏠 Press [h] to reset arms to home position (without exiting).") + if args.ki > 0.0: + logger_mp.info(f"🔧 Press [i] to toggle integral drift correction (Ki={args.ki}, clamp={args.i_clamp}m, decay={args.i_decay})") + else: + logger_mp.info("⚪ Integral correction DISABLED (--ki=0.0)") logger_mp.info("🔴 Press [q] to stop and exit the program.") logger_mp.info("⚠️ IMPORTANT: Please keep your distance and stay safe.") READY = True # now ready to (1) enter START state @@ -256,9 +274,34 @@ if __name__ == '__main__': logger_mp.info("---------------------🚀start Tracking🚀-------------------------") arm_ctrl.speed_gradual_max() + + # Integral drift correction state + i_error_left = np.zeros(3) # Accumulated task-space position error (meters) + i_error_right = np.zeros(3) + last_loop_time = time.time() + i_frame_count = 0 + # main loop. robot start to follow VR user's motion while not STOP: start_time = time.time() + + # handle arm reset request + if RESET_ARMS: + RESET_ARMS = False + logger_mp.info("[main] Resetting arms to home position...") + i_error_left = np.zeros(3) + i_error_right = np.zeros(3) + arm_ctrl.ctrl_dual_arm(np.zeros(14), np.zeros(14)) + reset_start = time.time() + while time.time() - reset_start < 3.0: + if np.all(np.abs(arm_ctrl.get_current_dual_arm_q()) < 0.1): + break + time.sleep(0.05) + logger_mp.info("[main] Arms reset complete, resuming tracking") + arm_ctrl.speed_gradual_max() + last_loop_time = time.time() + continue + # get image if camera_config['head_camera']['enable_zmq']: if args.record or xr_need_local_img: @@ -324,9 +367,47 @@ if __name__ == '__main__': current_lr_arm_q = arm_ctrl.get_current_dual_arm_q() current_lr_arm_dq = arm_ctrl.get_current_dual_arm_dq() - # solve ik using motor data and wrist pose, then use ik results to control arms. + # --- Integral drift correction --- + now = time.time() + dt = min(now - last_loop_time, 0.1) # clamp to prevent huge jumps after stalls + last_loop_time = now + + left_wrist_adjusted = tele_data.left_wrist_pose.copy() + right_wrist_adjusted = tele_data.right_wrist_pose.copy() + + if args.ki > 0.0 and INTEGRAL_ENABLED: + # FK: where are the robot hands actually? + left_ee_pos, right_ee_pos = arm_ik.compute_fk(current_lr_arm_q) + + # Error: target - actual (task-space position) + left_error = tele_data.left_wrist_pose[:3, 3] - left_ee_pos + right_error = tele_data.right_wrist_pose[:3, 3] - right_ee_pos + + # Leaky integrator with anti-windup + i_error_left = i_error_left * args.i_decay + left_error * dt + i_error_right = i_error_right * args.i_decay + right_error * dt + i_error_left = np.clip(i_error_left, -args.i_clamp, args.i_clamp) + i_error_right = np.clip(i_error_right, -args.i_clamp, args.i_clamp) + + # Bias IK target position (rotation unchanged) + left_wrist_adjusted[:3, 3] += args.ki * i_error_left + right_wrist_adjusted[:3, 3] += args.ki * i_error_right + + # Log every ~1 second (every 30 frames at 30Hz) + i_frame_count += 1 + if i_frame_count % 30 == 0: + logger_mp.info( + f"[I-term] L_err={np.linalg.norm(left_error):.4f}m " + f"R_err={np.linalg.norm(right_error):.4f}m | " + f"L_I={np.linalg.norm(i_error_left):.4f} " + f"R_I={np.linalg.norm(i_error_right):.4f} | " + f"L_corr={np.linalg.norm(args.ki * i_error_left):.4f}m " + f"R_corr={np.linalg.norm(args.ki * i_error_right):.4f}m" + ) + + # solve ik using motor data and adjusted wrist pose time_ik_start = time.time() - sol_q, sol_tauff = arm_ik.solve_ik(tele_data.left_wrist_pose, tele_data.right_wrist_pose, current_lr_arm_q, current_lr_arm_dq) + sol_q, sol_tauff = arm_ik.solve_ik(left_wrist_adjusted, right_wrist_adjusted, current_lr_arm_q, current_lr_arm_dq) time_ik_end = time.time() logger_mp.debug(f"ik:\t{round(time_ik_end - time_ik_start, 6)}") arm_ctrl.ctrl_dual_arm(sol_q, sol_tauff)