From 35af5bca7ce15d764574469ec85771d088acb377 Mon Sep 17 00:00:00 2001 From: Joe DiPrima Date: Wed, 18 Feb 2026 21:00:58 -0600 Subject: [PATCH] Replace EMA head smoothing with rotation-gated compensation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The EMA approach was mathematically flawed — it amplified stepping errors by (2-alpha) instead of filtering them. The new approach uses head angular velocity to distinguish head turns from body stepping: - Gate opens (compensate) when head rotation > 0.1 rad/s (looking around) - Gate stays closed (no compensation) when head is still (stepping) - Head position changes are only accumulated into compensation when the gate is open, so tv_wrapper's natural body-movement cancellation works correctly during stepping Also removes --head-smooth-alpha CLI arg (no longer needed). Co-Authored-By: Claude Opus 4.6 --- teleop/teleop_hand_and_arm.py | 49 +++++++++++++++++++++++------------ 1 file changed, 32 insertions(+), 17 deletions(-) diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index 0e50fa5..326cf78 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -156,8 +156,6 @@ if __name__ == '__main__': help='Seconds to ramp tracking after calibration (default: 0.5)') parser.add_argument('--rot-blend', type=float, default=0.7, help='Rotation tracking blend (0=position-only, 1=full rotation, default: 0.7)') - parser.add_argument('--head-smooth-alpha', type=float, default=0.02, - help='EMA alpha for head position smoothing (0=frozen, 1=no smoothing, default: 0.02)') # 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') @@ -356,7 +354,9 @@ if __name__ == '__main__': vp_ref_right = None robot_ref_left = None # 4x4: Robot FK pose at calibration moment robot_ref_right = None - head_smooth = None # 3-vec: EMA of head position (for decoupling) + head_rot_prev = None # 3x3: previous frame head rotation (for gating) + head_pos_prev = None # 3-vec: previous frame head position + head_compensation = np.zeros(3) # accumulated rotation-gated head position correction prev_start = False # Initial calibration: capture reference poses on first START @@ -366,13 +366,14 @@ if __name__ == '__main__': robot_ref_left, robot_ref_right = arm_ik.compute_fk_pose(_cal_arm_q) vp_ref_left = _cal_tele.left_wrist_pose.copy() vp_ref_right = _cal_tele.right_wrist_pose.copy() - head_smooth = _cal_tele.head_pose[:3, 3].copy() + head_rot_prev = _cal_tele.head_pose[:3, :3].copy() + head_pos_prev = _cal_tele.head_pose[:3, 3].copy() + head_compensation = np.zeros(3) calibrated = True calibration_time = time.time() prev_start = True logger_mp.info(f"[calibration] Initial reference captured. " - f"Robot L={robot_ref_left[:3,3]}, R={robot_ref_right[:3,3]} " - f"Head={head_smooth}") + f"Robot L={robot_ref_left[:3,3]}, R={robot_ref_right[:3,3]}") # main loop. robot start to follow VR user's motion while not STOP: @@ -386,6 +387,7 @@ if __name__ == '__main__': i_error_right = np.zeros(3) i_rot_error_left = np.zeros(3) i_rot_error_right = np.zeros(3) + head_compensation = 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: @@ -428,7 +430,9 @@ if __name__ == '__main__': robot_ref_left, robot_ref_right = arm_ik.compute_fk_pose(_cal_arm_q) vp_ref_left = _cal_tele.left_wrist_pose.copy() vp_ref_right = _cal_tele.right_wrist_pose.copy() - head_smooth = _cal_tele.head_pose[:3, 3].copy() + head_rot_prev = _cal_tele.head_pose[:3, :3].copy() + head_pos_prev = _cal_tele.head_pose[:3, 3].copy() + head_compensation = np.zeros(3) calibrated = True calibration_time = time.time() i_error_left = np.zeros(3) @@ -445,6 +449,7 @@ if __name__ == '__main__': i_error_right = np.zeros(3) i_rot_error_left = np.zeros(3) i_rot_error_right = np.zeros(3) + head_compensation = np.zeros(3) current_time = time.time() time_elapsed = current_time - start_time sleep_time = max(0, (1 / args.frequency) - time_elapsed) @@ -527,16 +532,26 @@ if __name__ == '__main__': settle_elapsed = now - calibration_time settle_alpha = min(1.0, settle_elapsed / max(args.settle_time, 0.01)) - # Head decoupling via EMA: tv_wrapper subtracts live head position from wrists. - # We compensate only FAST head movement (looking around), not slow body movement - # (stepping). head_smooth tracks body position; head_fast = head_now - head_smooth. - head_smooth = (head_smooth * (1 - args.head_smooth_alpha) - + tele_data.head_pose[:3, 3] * args.head_smooth_alpha) - head_fast = tele_data.head_pose[:3, 3] - head_smooth - - # Position: robot_ref + settle_alpha * (vp_current + head_fast - vp_ref) - left_delta_pos = (tele_data.left_wrist_pose[:3, 3] + head_fast) - vp_ref_left[:3, 3] - right_delta_pos = (tele_data.right_wrist_pose[:3, 3] + head_fast) - vp_ref_right[:3, 3] + # Head decoupling via rotation gating: only compensate head position + # drift caused by head ROTATION (looking around), not body translation + # (stepping). When head rotates, gate opens and position change is accumulated. + # When head is still, gate stays closed so tv_wrapper's natural body-movement + # cancellation works correctly. + head_rot_now = tele_data.head_pose[:3, :3] + head_pos_now = tele_data.head_pose[:3, 3] + if head_rot_prev is not None: + rot_delta = rotation_to_rotvec(head_rot_now @ head_rot_prev.T) + angular_speed = np.linalg.norm(rot_delta) / max(dt, 0.001) + # Gate: 0 when head still (stepping), 1 when rotating (looking around) + gate = np.clip((angular_speed - 0.1) / 0.3, 0.0, 1.0) + pos_delta = head_pos_now - head_pos_prev + head_compensation += gate * pos_delta + head_rot_prev = head_rot_now.copy() + head_pos_prev = head_pos_now.copy() + + # Position: robot_ref + settle_alpha * (vp_current + head_comp - vp_ref) + left_delta_pos = (tele_data.left_wrist_pose[:3, 3] + head_compensation) - vp_ref_left[:3, 3] + right_delta_pos = (tele_data.right_wrist_pose[:3, 3] + head_compensation) - vp_ref_right[:3, 3] left_wrist_adjusted = np.eye(4) right_wrist_adjusted = np.eye(4)