epilectrik
/
teleop
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Fix calibration jerk and wrist rotation drift 

1. Settle ramp: after calibration, smoothly blend VP deltas from 0%
   to 100% over --settle-time (default 0.5s). Prevents the initial
   arm jerk caused by VP tracking jitter on the first frame.

2. Rotation blend: scale VP rotation deltas by --rot-blend (default
   0.3) to prevent wrist joints from drifting to odd positions.
   The VP-to-Unitree rotation convention may be wrong for Vision Pro
   (tuned for PICO), so dampening rotation tracking avoids the IK
   solver fighting a bad rotation target. Use --rot-blend 0 for
   position-only tracking, --rot-blend 1 for full rotation.

Both parameters ramp together during settle: rot_alpha = rot_blend * settle_alpha

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
main
Joe DiPrima 1 month ago
parent
206830c530
commit
4c6bf4f67b
1 changed files with 45 additions and 12 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 57
      teleop/teleop_hand_and_arm.py

57
teleop/teleop_hand_and_arm.py
View File

@ -66,6 +66,23 @@ def parse_r3_buttons(data):
'Start': bool(btn1 & 0x04),
}
def scale_rotation(R, alpha):
"""Scale a rotation matrix's angle by alpha (0=identity, 1=original). Pure numpy."""
cos_theta = np.clip((np.trace(R) - 1.0) / 2.0, -1.0, 1.0)
theta = np.arccos(cos_theta)
if theta < 1e-6:
return np.eye(3)
axis = np.array([
R[2, 1] - R[1, 2],
R[0, 2] - R[2, 0],
R[1, 0] - R[0, 1]
]) / (2.0 * np.sin(theta))
scaled = theta * alpha
K = np.array([[0, -axis[2], axis[1]],
[axis[2], 0, -axis[0]],
[-axis[1], axis[0], 0]])
return np.eye(3) + np.sin(scaled) * K + (1.0 - np.cos(scaled)) * (K @ K)
# Previous button state for edge detection
_r3_prev_buttons = {}
@ -120,6 +137,10 @@ if __name__ == '__main__':
parser.add_argument('--i-decay', type=float, default=0.995, help='Per-frame decay factor for I term (default: 0.995)')
parser.add_argument('--no-calibration', action='store_true',
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)')
# 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')
@ -311,6 +332,7 @@ if __name__ == '__main__':
# Relative-mode calibration state
calibrated = False
calibration_time = 0.0 # When calibration was captured (for settle ramp)
vp_ref_left = None # 4x4: VP wrist pose at calibration moment
vp_ref_right = None
robot_ref_left = None # 4x4: Robot FK pose at calibration moment
@ -325,6 +347,7 @@ if __name__ == '__main__':
vp_ref_left = _cal_tele.left_wrist_pose.copy()
vp_ref_right = _cal_tele.right_wrist_pose.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]}")
@ -382,6 +405,7 @@ if __name__ == '__main__':
vp_ref_left = _cal_tele.left_wrist_pose.copy()
vp_ref_right = _cal_tele.right_wrist_pose.copy()
calibrated = True
calibration_time = time.time()
i_error_left = np.zeros(3)
i_error_right = np.zeros(3)
logger_mp.info(f"[calibration] Re-calibrated on resume. "
@ -470,20 +494,29 @@ if __name__ == '__main__':
# --- Apply relative-mode calibration ---
if calibrated:
# Position: robot_ref + (vp_current - vp_ref)
# Settle ramp: smoothly blend from 0% to 100% delta over settle_time
settle_elapsed = now - calibration_time
settle_alpha = min(1.0, settle_elapsed / max(args.settle_time, 0.01))
# Position: robot_ref + settle_alpha * (vp_current - vp_ref)
left_delta_pos = tele_data.left_wrist_pose[:3, 3] - vp_ref_left[:3, 3]
right_delta_pos = tele_data.right_wrist_pose[:3, 3] - vp_ref_right[:3, 3]
left_wrist_adjusted = np.eye(4)
right_wrist_adjusted = np.eye(4)
left_wrist_adjusted[:3, 3] = robot_ref_left[:3, 3] + (
tele_data.left_wrist_pose[:3, 3] - vp_ref_left[:3, 3])
right_wrist_adjusted[:3, 3] = robot_ref_right[:3, 3] + (
tele_data.right_wrist_pose[:3, 3] - vp_ref_right[:3, 3])
# Rotation: delta_R @ robot_ref_R where delta_R = vp_R @ vp_ref_R^T
left_wrist_adjusted[:3, :3] = (
tele_data.left_wrist_pose[:3, :3] @ vp_ref_left[:3, :3].T
) @ robot_ref_left[:3, :3]
right_wrist_adjusted[:3, :3] = (
tele_data.right_wrist_pose[:3, :3] @ vp_ref_right[:3, :3].T
) @ robot_ref_right[:3, :3]
left_wrist_adjusted[:3, 3] = robot_ref_left[:3, 3] + settle_alpha * left_delta_pos
right_wrist_adjusted[:3, 3] = robot_ref_right[:3, 3] + settle_alpha * right_delta_pos
# Rotation: compute delta, scale by rot_blend (and settle_alpha)
left_delta_R = tele_data.left_wrist_pose[:3, :3] @ vp_ref_left[:3, :3].T
right_delta_R = tele_data.right_wrist_pose[:3, :3] @ vp_ref_right[:3, :3].T
rot_alpha = args.rot_blend * settle_alpha
if rot_alpha < 0.01:
left_wrist_adjusted[:3, :3] = robot_ref_left[:3, :3]
right_wrist_adjusted[:3, :3] = robot_ref_right[:3, :3]
else:
left_wrist_adjusted[:3, :3] = scale_rotation(left_delta_R, rot_alpha) @ robot_ref_left[:3, :3]
right_wrist_adjusted[:3, :3] = scale_rotation(right_delta_R, rot_alpha) @ robot_ref_right[:3, :3]
else:
left_wrist_adjusted = tele_data.left_wrist_pose.copy()
right_wrist_adjusted = tele_data.right_wrist_pose.copy()

Write Preview
Loading…
Cancel
Save