diff --git a/teleop/teleop_hand_and_arm.py b/teleop/teleop_hand_and_arm.py index e0b9bcb..0e50fa5 100644 --- a/teleop/teleop_hand_and_arm.py +++ b/teleop/teleop_hand_and_arm.py @@ -154,8 +154,10 @@ if __name__ == '__main__': help='Disable start-time calibration (use absolute VP poses)') parser.add_argument('--settle-time', type=float, default=0.5, help='Seconds to ramp tracking after calibration (default: 0.5)') - parser.add_argument('--rot-blend', type=float, default=0.3, - help='Rotation tracking blend (0=position-only, 1=full rotation, default: 0.3)') + 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') @@ -354,7 +356,7 @@ if __name__ == '__main__': vp_ref_right = None robot_ref_left = None # 4x4: Robot FK pose at calibration moment robot_ref_right = None - head_ref = None # 3-vec: Head position at calibration (for decoupling) + head_smooth = None # 3-vec: EMA of head position (for decoupling) prev_start = False # Initial calibration: capture reference poses on first START @@ -364,13 +366,13 @@ 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_ref = _cal_tele.head_pose[:3, 3].copy() + head_smooth = _cal_tele.head_pose[:3, 3].copy() 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_ref}") + f"Head={head_smooth}") # main loop. robot start to follow VR user's motion while not STOP: @@ -426,7 +428,7 @@ 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_ref = _cal_tele.head_pose[:3, 3].copy() + head_smooth = _cal_tele.head_pose[:3, 3].copy() calibrated = True calibration_time = time.time() i_error_left = np.zeros(3) @@ -525,14 +527,16 @@ if __name__ == '__main__': settle_elapsed = now - calibration_time settle_alpha = min(1.0, settle_elapsed / max(args.settle_time, 0.01)) - # Head decoupling: tv_wrapper subtracts live head position from wrists. - # Add back head drift so only hand movement (not head movement) drives arms. - head_drift = tele_data.head_pose[:3, 3] - head_ref + # 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_drift - vp_ref) - # head_drift cancels tv_wrapper's live head subtraction, replacing it with fixed ref - left_delta_pos = (tele_data.left_wrist_pose[:3, 3] + head_drift) - vp_ref_left[:3, 3] - right_delta_pos = (tele_data.right_wrist_pose[:3, 3] + head_drift) - vp_ref_right[:3, 3] + # 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] left_wrist_adjusted = np.eye(4) right_wrist_adjusted = np.eye(4)