@ -53,10 +53,16 @@ if not hasattr(dex_retargeting, 'RetargetingConfig'):
from dex_retargeting.retargeting_config import RetargetingConfig from dex_retargeting.retargeting_config import RetargetingConfig
dex_retargeting . RetargetingConfig = RetargetingConfig dex_retargeting . RetargetingConfig = RetargetingConfig
# IK solver from upstream xr_teleoperate
sys . path . insert ( 0 , os . path . join ( XR_TELEOP_DIR , " teleop " , " robot_control " ) )
from arm_ik_elbow import G1_29_ArmIK_Elbow
# Local imports # Local imports
from teleop_server import TeleopServer from teleop_server import TeleopServer
from native_tv_wrapper import ( T_ROBOT_OPENXR , T_OPENXR_ROBOT , from native_tv_wrapper import ( T_ROBOT_OPENXR , T_OPENXR_ROBOT ,
T_TO_UNITREE_HAND , T_TO_UNITREE_HAND ,
T_TO_UNITREE_HUMANOID_LEFT_ARM ,
T_TO_UNITREE_HUMANOID_RIGHT_ARM ,
NativeTeleWrapper ) NativeTeleWrapper )
logger = logging . getLogger ( " retarget_bridge " ) logger = logging . getLogger ( " retarget_bridge " )
@ -142,23 +148,8 @@ R3_OPENXR_ROBOT = T_OPENXR_ROBOT[:3, :3]
JOINT_SCALE = np . ones ( 17 ) JOINT_SCALE = np . ones ( 17 )
JOINT_OFFSET = np . zeros ( 17 ) JOINT_OFFSET = np . zeros ( 17 )
# l_shoulder_pitch (index 3): XR range [-0.4, +2.3] -> robot range [-2.4, +0.8]
JOINT_SCALE [ 3 ] = 1.2
JOINT_OFFSET [ 3 ] = - 1.95
# l_shoulder_roll (index 4): XR Z range [+0.06, +1.05] -> robot range [0, +1.57]
JOINT_SCALE [ 4 ] = 1.6
JOINT_OFFSET [ 4 ] = - 0.1
# l_shoulder_yaw (index 5): XR -X range [-0.24, +0.93] -> robot range [-1.14, +1.14]
JOINT_SCALE [ 5 ] = 2.0
JOINT_OFFSET [ 5 ] = - 0.7
# l_elbow (index 6): XR Z range [-0.67, +0.97] -> robot range [-0.78, +1.31]
JOINT_SCALE [ 6 ] = 1.3
JOINT_OFFSET [ 6 ] = 0.0
# TODO: calibrate remaining joints (wrist, right arm)
# All arm joints use IK solver output directly — no scaling needed
# JOINT_SCALE stays at 1.0 for all joints
# Smoothing parameters # Smoothing parameters
EMA_ALPHA = 0.4 # Exponential moving average (0=no update, 1=no smoothing) EMA_ALPHA = 0.4 # Exponential moving average (0=no update, 1=no smoothing)
@ -203,6 +194,55 @@ def all_joints_valid(bj):
' r_upper ' , ' r_lower ' , ' r_wrist ' ] ) ' r_upper ' , ' r_lower ' , ' r_wrist ' ] )
def build_ik_targets ( bj ) :
""" Build IK-ready wrist SE(3) poses + elbow positions from body tracking.
Uses hips as waist reference . Wrist rotation is preserved ( from XRHandTracker )
to help the IK solver resolve elbow ambiguity . Elbow positions come from
XRBodyTracker l_lower / r_lower joints .
Args :
bj : dict from parse_body_joints ( ) — world - space 4 x4 matrices
Returns :
( left_wrist_4x4 , right_wrist_4x4 , left_elbow_pos , right_elbow_pos )
Wrists are 4 x4 SE ( 3 ) in robot waist frame .
Elbows are 3 - vectors ( xyz ) in robot waist frame .
"""
l_wrist_world = bj [ ' l_wrist ' ]
r_wrist_world = bj [ ' r_wrist ' ]
hips_world = bj [ ' hips ' ]
# Basis change: OpenXR → Robot convention
hips_robot = T_ROBOT_OPENXR @ hips_world @ T_OPENXR_ROBOT
left_Brobot_world = T_ROBOT_OPENXR @ l_wrist_world @ T_OPENXR_ROBOT
right_Brobot_world = T_ROBOT_OPENXR @ r_wrist_world @ T_OPENXR_ROBOT
# Arm URDF convention transform (for wrists only — adjusts end-effector frame)
left_IPunitree = left_Brobot_world @ T_TO_UNITREE_HUMANOID_LEFT_ARM
right_IPunitree = right_Brobot_world @ T_TO_UNITREE_HUMANOID_RIGHT_ARM
# Hips subtraction (translation only — makes wrist relative to waist)
hips_pos = hips_robot [ 0 : 3 , 3 ]
left_wrist_final = left_IPunitree . copy ( )
right_wrist_final = right_IPunitree . copy ( )
left_wrist_final [ 0 : 3 , 3 ] - = hips_pos
right_wrist_final [ 0 : 3 , 3 ] - = hips_pos
# Elbow positions: basis change + hips subtraction (no arm convention transform)
l_elbow_pos = None
r_elbow_pos = None
if bj [ ' l_lower ' ] is not None :
l_lower_robot = T_ROBOT_OPENXR @ bj [ ' l_lower ' ] @ T_OPENXR_ROBOT
l_elbow_pos = l_lower_robot [ 0 : 3 , 3 ] - hips_pos
if bj [ ' r_lower ' ] is not None :
r_lower_robot = T_ROBOT_OPENXR @ bj [ ' r_lower ' ] @ T_OPENXR_ROBOT
r_elbow_pos = r_lower_robot [ 0 : 3 , 3 ] - hips_pos
return left_wrist_final , right_wrist_final , l_elbow_pos , r_elbow_pos
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
# Calibration: captures neutral pose reference rotations # Calibration: captures neutral pose reference rotations
# --------------------------------------------------------------------------- # ---------------------------------------------------------------------------
@ -218,10 +258,10 @@ class CalibrationData:
def __init__ ( self ) : def __init__ ( self ) :
self . calibrated = False self . calibrated = False
# Reference relative rotations (3x3 matrices) in XR parent-local frame
self . ref_waist = None # hips.T @ chest
self . ref_shoulder_L = None # chest.T @ l_upper
self . ref_shoulder_R = None # chest.T @ r_upper
self . ref_hips_rot = None # frozen hips rotation
self . ref_chest_rot = None # calibration-time chest rotation
self . ref_shoulder_L = None # chest.T @ l_upper (chest-relative)
self . ref_shoulder_R = None # chest.T @ r_upper (chest-relative)
self . ref_elbow_L = None # l_upper.T @ l_lower self . ref_elbow_L = None # l_upper.T @ l_lower
self . ref_elbow_R = None # r_upper.T @ r_lower self . ref_elbow_R = None # r_upper.T @ r_lower
self . ref_wrist_L = None # l_lower.T @ l_wrist self . ref_wrist_L = None # l_lower.T @ l_wrist
@ -239,9 +279,10 @@ class CalibrationData:
if not all_joints_valid ( bj ) : if not all_joints_valid ( bj ) :
return False return False
self . ref_waist = bj [ ' hips ' ] [ : 3 , : 3 ] . T @ bj [ ' chest ' ] [ : 3 , : 3 ]
self . ref_shoulder_L = bj [ ' hips ' ] [ : 3 , : 3 ] . T @ bj [ ' l_upper ' ] [ : 3 , : 3 ]
self . ref_shoulder_R = bj [ ' hips ' ] [ : 3 , : 3 ] . T @ bj [ ' r_upper ' ] [ : 3 , : 3 ]
self . ref_hips_rot = bj [ ' hips ' ] [ : 3 , : 3 ] . copy ( )
self . ref_chest_rot = bj [ ' chest ' ] [ : 3 , : 3 ] . copy ( )
self . ref_shoulder_L = self . ref_hips_rot . T @ bj [ ' l_upper ' ] [ : 3 , : 3 ]
self . ref_shoulder_R = self . ref_hips_rot . T @ bj [ ' r_upper ' ] [ : 3 , : 3 ]
self . ref_elbow_L = bj [ ' l_upper ' ] [ : 3 , : 3 ] . T @ bj [ ' l_lower ' ] [ : 3 , : 3 ] self . ref_elbow_L = bj [ ' l_upper ' ] [ : 3 , : 3 ] . T @ bj [ ' l_lower ' ] [ : 3 , : 3 ]
self . ref_elbow_R = bj [ ' r_upper ' ] [ : 3 , : 3 ] . T @ bj [ ' r_lower ' ] [ : 3 , : 3 ] self . ref_elbow_R = bj [ ' r_upper ' ] [ : 3 , : 3 ] . T @ bj [ ' r_lower ' ] [ : 3 , : 3 ]
self . ref_wrist_L = bj [ ' l_lower ' ] [ : 3 , : 3 ] . T @ bj [ ' l_wrist ' ] [ : 3 , : 3 ] self . ref_wrist_L = bj [ ' l_lower ' ] [ : 3 , : 3 ] . T @ bj [ ' l_wrist ' ] [ : 3 , : 3 ]
@ -276,6 +317,7 @@ def _joint_delta_robot(R_ref, R_cur):
return R3_ROBOT_OPENXR @ R_delta @ R3_ROBOT_OPENXR . T return R3_ROBOT_OPENXR @ R_delta @ R3_ROBOT_OPENXR . T
_last_dbg = { } # Raw euler values for debug logging
SOLO_JOINT = None # Set by --solo-joint arg; None = all joints active SOLO_JOINT = None # Set by --solo-joint arg; None = all joints active
# Map solo-joint names to indices within the 17-element angles array # Map solo-joint names to indices within the 17-element angles array
@ -310,40 +352,42 @@ def compute_all_angles(bj, cal):
""" """
angles = np . zeros ( 17 ) # indices 0-16 map to joints 12-28 angles = np . zeros ( 17 ) # indices 0-16 map to joints 12-28
# Axis mappings determined empirically via joint_calibrate.py:
#
# After R_basis @ R_delta @ R_basis.T, as_euler('YXZ') returns (Y, X, Z).
# The mapping to robot joints depends on the parent's orientation:
#
# Shoulder (parent=chest, upright): pitch=Y[0], roll=Z[2], yaw=-X[1]
# Elbow (parent=upper_arm, hanging): pitch=Z[2]
# Waist (parent=hips, upright): assumed same pattern as shoulder
# Wrist (parent=lower_arm, hanging): assumed same pattern as elbow
# --- Waist (0-2): DISABLED (not calibrated) --- # --- Waist (0-2): DISABLED (not calibrated) ---
# angles[0:3] = [0, 0, 0] # angles[0:3] = [0, 0, 0]
# --- Left shoulder (3-5): hips -> l_upper ---
# Using hips instead of chest to avoid head rotation contamination
# Parent (hips) is upright: pitch=Y, roll=Z, yaw=-X
R_sh_L_cur = bj [ ' hips ' ] [ : 3 , : 3 ] . T @ bj [ ' l_upper ' ] [ : 3 , : 3 ]
# --- Left shoulder (3-5): frozen_hips -> l_upper ---
R_sh_L_cur = cal . ref_hips_rot . T @ bj [ ' l_upper ' ] [ : 3 , : 3 ]
R_sd_L = _joint_delta_robot ( cal . ref_shoulder_L , R_sh_L_cur ) R_sd_L = _joint_delta_robot ( cal . ref_shoulder_L , R_sh_L_cur )
sy , sx , sz = Rotation . from_matrix ( R_sd_L ) . as_euler ( ' YXZ ' ) sy , sx , sz = Rotation . from_matrix ( R_sd_L ) . as_euler ( ' YXZ ' )
angles [ 3 : 6 ] = [ sy , sz , - sx ] # pitch=Y, roll=Z, yaw=-X
# --- Left elbow (6): l_upper -> l_lower --- # --- Left elbow (6): l_upper -> l_lower ---
# Parent (upper arm) hangs down: pitch=Z
R_el_L_cur = bj [ ' l_upper ' ] [ : 3 , : 3 ] . T @ bj [ ' l_lower ' ] [ : 3 , : 3 ] R_el_L_cur = bj [ ' l_upper ' ] [ : 3 , : 3 ] . T @ bj [ ' l_lower ' ] [ : 3 , : 3 ]
R_ed_L = _joint_delta_robot ( cal . ref_elbow_L , R_el_L_cur ) R_ed_L = _joint_delta_robot ( cal . ref_elbow_L , R_el_L_cur )
_ , _ , ez_L = Rotation . from_matrix ( R_ed_L ) . as_euler ( ' YXZ ' )
angles [ 6 ] = ez_L # pitch = Z component
ey_L , ex_L , ez_L = Rotation . from_matrix ( R_ed_L ) . as_euler ( ' YXZ ' )
# Full matrix mapping (from pose_calibrate.py 2026-02-28)
# Solves cross-axis coupling + scales in one step
# robot = M @ [sy, sx, sz, ez]
M_LEFT = np . array ( [
[ + 5.4026 , - 1.7709 , + 1.0419 , - 6.7338 ] , # pitch
[ + 4.4651 , - 0.0869 , + 2.8320 , - 5.4197 ] , # roll
[ + 3.1454 , - 1.6180 , + 2.3173 , - 3.9653 ] , # yaw
[ + 0.6962 , + 1.7176 , + 1.5304 , + 1.0564 ] , # elbow
] )
xr_vec = np . array ( [ sy , sx , sz , ez_L ] )
pitch , roll , yaw , elbow = M_LEFT @ xr_vec
angles [ 3 : 6 ] = [ pitch , roll , yaw ]
angles [ 6 ] = elbow
_last_dbg [ ' sh_YXZ ' ] = ( sy , sx , sz )
_last_dbg [ ' el_YXZ ' ] = ( ey_L , ex_L , ez_L )
# --- Left wrist (7-9): l_lower -> l_wrist --- # --- Left wrist (7-9): l_lower -> l_wrist ---
# Parent (lower arm) hangs down: same pattern as elbow (Z-dominant) # Parent (lower arm) hangs down: same pattern as elbow (Z-dominant)
R_wr_L_cur = bj [ ' l_lower ' ] [ : 3 , : 3 ] . T @ bj [ ' l_wrist ' ] [ : 3 , : 3 ] R_wr_L_cur = bj [ ' l_lower ' ] [ : 3 , : 3 ] . T @ bj [ ' l_wrist ' ] [ : 3 , : 3 ]
R_wrd_L = _joint_delta_robot ( cal . ref_wrist_L , R_wr_L_cur ) R_wrd_L = _joint_delta_robot ( cal . ref_wrist_L , R_wr_L_cur )
wy_wl , wx_wl , wz_wl = Rotation . from_matrix ( R_wrd_L ) . as_euler ( ' YXZ ' ) wy_wl , wx_wl , wz_wl = Rotation . from_matrix ( R_wrd_L ) . as_euler ( ' YXZ ' )
angles [ 7 : 10 ] = [ wz_wl , wy_wl , - wx_wl ] # roll=Z, pitch=Y, yaw=-X (tentative)
angles [ 7 : 10 ] = [ 0.0 , 0.0 , 0.0 ] # all wrist joints disabled for now (noisy )
# --- Right arm (10-16): DISABLED (not calibrated, Quest tracking weak) --- # --- Right arm (10-16): DISABLED (not calibrated, Quest tracking weak) ---
# angles[10:17] = [0, 0, 0, 0, 0, 0, 0] # angles[10:17] = [0, 0, 0, 0, 0, 0, 0]
@ -591,6 +635,14 @@ def main():
if hand_data : if hand_data :
l_retarget , r_retarget , l_indices , r_indices , l_hw_map , r_hw_map = hand_data l_retarget , r_retarget , l_indices , r_indices , l_hw_map , r_hw_map = hand_data
# --- Initialize IK solver ---
logger . info ( " Initializing IK solver (Pinocchio + CasADi)... " )
orig_cwd = os . getcwd ( )
os . chdir ( os . path . join ( XR_TELEOP_DIR , " teleop " ) )
arm_ik = G1_29_ArmIK_Elbow ( Unit_Test = False , Visualization = False )
os . chdir ( orig_cwd )
logger . info ( " IK solver ready (14 DOF: 7 left arm + 7 right arm, elbow constraints) " )
# --- Control loop state --- # --- Control loop state ---
interval = 1.0 / args . hz interval = 1.0 / args . hz
max_step = MAX_RATE * interval # rad per step max_step = MAX_RATE * interval # rad per step
@ -598,7 +650,7 @@ def main():
current_angles = np . zeros ( NUM_JOINTS ) current_angles = np . zeros ( NUM_JOINTS )
smoothed_upper = np . zeros ( 17 ) # joints 12-28 smoothed_upper = np . zeros ( 17 ) # joints 12-28
current_hands = np . ones ( 12 ) current_hands = np . ones ( 12 )
tracking_timeout = 0.5
tracking_timeout = 2.0 # seconds without data before considering tracking lost
return_to_zero_rate = 0.02 return_to_zero_rate = 0.02
last_valid_time = 0.0 last_valid_time = 0.0
startup_ramp = 0.0 startup_ramp = 0.0
@ -619,11 +671,11 @@ def main():
signal . signal ( signal . SIGINT , signal_handler ) signal . signal ( signal . SIGINT , signal_handler )
signal . signal ( signal . SIGTERM , signal_handler ) signal . signal ( signal . SIGTERM , signal_handler )
logger . info ( f " Bridge v4 running at {args.hz} Hz (DIRECT retargeting, no IK ) " )
logger . info ( f " Bridge v5 running at {args.hz} Hz (IK retargeting via Pinocchio ) " )
logger . info ( f " Waist={ ' ON ' if not args.no_waist else ' OFF ' }, " logger . info ( f " Waist={ ' ON ' if not args.no_waist else ' OFF ' }, "
f " Arms={ ' ON ' if not args.no_arms else ' OFF ' }, " f " Arms={ ' ON ' if not args.no_arms else ' OFF ' }, "
f " Hands={ ' ON ' if hand_data else ' OFF ' } " ) f " Hands={ ' ON ' if hand_data else ' OFF ' } " )
logger . info ( f " Auto-calibration delay: {args.cal_delay}s after first tracking frame " )
logger . info ( f " Startup delay: {args.cal_delay}s after first tracking frame " )
logger . info ( " Waiting for Quest 3... " ) logger . info ( " Waiting for Quest 3... " )
while running : while running :
@ -650,12 +702,7 @@ def main():
# Detect reconnection: tracking resumed after being lost # Detect reconnection: tracking resumed after being lost
if not was_tracking and cal . calibrated : if not was_tracking and cal . calibrated :
logger . info ( " Tracking resumed after disconnect — RESETTING calibration " )
cal = CalibrationData ( )
first_data_time = now
smoothed_upper = np . zeros ( 17 )
startup_ramp = 0.0
startup_time = None
logger . info ( " Tracking resumed — keeping existing calibration " )
was_tracking = True was_tracking = True
@ -664,34 +711,61 @@ def main():
elapsed_startup = now - startup_time elapsed_startup = now - startup_time
startup_ramp = min ( elapsed_startup / startup_ramp_duration , 1.0 ) startup_ramp = min ( elapsed_startup / startup_ramp_duration , 1.0 )
# --- Parse body joints ---
if has_body_joints :
bj = parse_body_joints ( bj_raw )
# --- Auto-calibrate after delay ---
if not cal . calibrated and first_data_time is not None :
if ( now - first_data_time ) > = args . cal_delay :
if all_joints_valid ( bj ) :
if cal . capture ( bj ) :
logger . info ( " CALIBRATED — neutral pose captured. Starting retargeting. " )
# Log calibration reference quaternions for debugging
l_up = bj_raw [ BJ_L_UPPER * 7 : ( BJ_L_UPPER + 1 ) * 7 ]
r_up = bj_raw [ BJ_R_UPPER * 7 : ( BJ_R_UPPER + 1 ) * 7 ]
logger . info ( f " Cal ref L_upper quat=[{l_up[3]:.4f},{l_up[4]:.4f},{l_up[5]:.4f},{l_up[6]:.4f}] " )
logger . info ( f " Cal ref R_upper quat=[{r_up[3]:.4f},{r_up[4]:.4f},{r_up[5]:.4f},{r_up[6]:.4f}] " )
startup_time = now
else :
logger . warning ( " Calibration capture failed, retrying next frame... " )
else :
missing = [ k for k in [ ' hips ' , ' chest ' , ' l_upper ' , ' l_lower ' , ' l_wrist ' ,
' r_upper ' , ' r_lower ' , ' r_wrist ' ] if bj [ k ] is None ]
if step_count % 50 == 0 :
logger . info ( f " Waiting for all body joints (missing: {missing}) " )
# --- Retarget body joints ---
if cal . calibrated and all_joints_valid ( bj ) :
try :
raw_angles = compute_all_angles ( bj , cal )
# --- Start IK retargeting after delay ---
if not cal . calibrated and first_data_time is not None :
if ( now - first_data_time ) > = args . cal_delay :
cal . calibrated = True # IK doesn't need body joint calibration
logger . info ( " Startup delay complete — IK retargeting active. " )
startup_time = now
# --- IK retarget: wrist poses -> joint angles ---
if cal . calibrated and has_body_joints :
try :
bj = parse_body_joints ( bj_raw )
if ( bj [ ' l_wrist ' ] is not None and bj [ ' r_wrist ' ] is not None
and bj [ ' hips ' ] is not None ) :
# Build IK targets: wrist SE(3) + elbow positions
lw , rw , l_elbow , r_elbow = build_ik_targets ( bj )
# Debug: log IK targets every 250 steps
if step_count % 250 == 0 :
l_elb_str = f " [{l_elbow[0]:+.3f},{l_elbow[1]:+.3f},{l_elbow[2]:+.3f}] " if l_elbow is not None else " None "
r_elb_str = f " [{r_elbow[0]:+.3f},{r_elbow[1]:+.3f},{r_elbow[2]:+.3f}] " if r_elbow is not None else " None "
# Raw OpenXR positions (Y-up, -Z forward)
lw_raw = bj [ ' l_wrist ' ] [ 0 : 3 , 3 ] if bj [ ' l_wrist ' ] is not None else np . zeros ( 3 )
rw_raw = bj [ ' r_wrist ' ] [ 0 : 3 , 3 ] if bj [ ' r_wrist ' ] is not None else np . zeros ( 3 )
le_raw = bj [ ' l_lower ' ] [ 0 : 3 , 3 ] if bj [ ' l_lower ' ] is not None else np . zeros ( 3 )
re_raw = bj [ ' r_lower ' ] [ 0 : 3 , 3 ] if bj [ ' r_lower ' ] is not None else np . zeros ( 3 )
hips_raw = bj [ ' hips ' ] [ 0 : 3 , 3 ] if bj [ ' hips ' ] is not None else np . zeros ( 3 )
logger . info ( f " [RAW OpenXR] hips=[{hips_raw[0]:+.3f},{hips_raw[1]:+.3f},{hips_raw[2]:+.3f}] " )
logger . info ( f " [RAW OpenXR] L_wrist=[{lw_raw[0]:+.3f},{lw_raw[1]:+.3f},{lw_raw[2]:+.3f}] "
f " L_elbow=[{le_raw[0]:+.3f},{le_raw[1]:+.3f},{le_raw[2]:+.3f}] " )
logger . info ( f " [RAW OpenXR] R_wrist=[{rw_raw[0]:+.3f},{rw_raw[1]:+.3f},{rw_raw[2]:+.3f}] "
f " R_elbow=[{re_raw[0]:+.3f},{re_raw[1]:+.3f},{re_raw[2]:+.3f}] " )
logger . info ( f " [IK INPUT] L_wrist pos=[{lw[0,3]:+.3f},{lw[1,3]:+.3f},{lw[2,3]:+.3f}] "
f " R_wrist pos=[{rw[0,3]:+.3f},{rw[1,3]:+.3f},{rw[2,3]:+.3f}] " )
logger . info ( f " [IK INPUT] L_elbow={l_elb_str} R_elbow={r_elb_str} " )
# Get current arm joint angles for warm-starting the IK solver
current_arm_q = np . concatenate ( [
current_angles [ 15 : 22 ] , # left arm (joints 15-21)
current_angles [ 22 : 29 ] , # right arm (joints 22-28)
] )
current_arm_dq = np . zeros ( 14 )
sol_q , sol_tauff = arm_ik . solve_ik (
lw , rw ,
current_arm_q ,
current_arm_dq ,
left_elbow_pos = l_elbow ,
right_elbow_pos = r_elbow ,
)
# Map 14-DOF IK solution to 17-element upper body array
raw_angles = np . zeros ( 17 )
raw_angles [ 3 : 10 ] = sol_q [ : 7 ] # left arm
raw_angles [ 10 : 17 ] = sol_q [ 7 : ] # right arm
raw_angles = apply_limits ( raw_angles ) raw_angles = apply_limits ( raw_angles )
# Zero out disabled groups # Zero out disabled groups
@ -706,10 +780,10 @@ def main():
# Apply startup ramp and write to output # Apply startup ramp and write to output
current_angles [ 12 : 29 ] = smoothed_upper * startup_ramp current_angles [ 12 : 29 ] = smoothed_upper * startup_ramp
except Exception as e :
retarget_error_count + = 1
if retarget_error_count < = 5 or retarget_error_count % 100 == 0 :
logger . warning ( f " Retarget failed ({retarget_error_count}x): {e}" )
except Exception as e :
retarget_error_count + = 1
if retarget_error_count < = 5 or retarget_error_count % 100 == 0 :
logger . warning ( f " IK solve failed ({retarget_error_count}x): {e}" )
# --- Hand retargeting --- (DISABLED: testing left arm only) # --- Hand retargeting --- (DISABLED: testing left arm only)
if False and hand_data and cal . calibrated : if False and hand_data and cal . calibrated :
@ -765,6 +839,12 @@ def main():
f " waist=[{waist[0]:+.2f},{waist[1]:+.2f},{waist[2]:+.2f}] " f " waist=[{waist[0]:+.2f},{waist[1]:+.2f},{waist[2]:+.2f}] "
f " L_sh=[{l_sh[0]:+.2f},{l_sh[1]:+.2f},{l_sh[2]:+.2f}] L_el={l_el:+.2f} " f " L_sh=[{l_sh[0]:+.2f},{l_sh[1]:+.2f},{l_sh[2]:+.2f}] L_el={l_el:+.2f} "
f " R_sh=[{r_sh[0]:+.2f},{r_sh[1]:+.2f},{r_sh[2]:+.2f}] R_el={r_el:+.2f} " ) f " R_sh=[{r_sh[0]:+.2f},{r_sh[1]:+.2f},{r_sh[2]:+.2f}] R_el={r_el:+.2f} " )
if tracking and cal . calibrated :
l_wr = current_angles [ 19 : 22 ]
r_wr = current_angles [ 26 : 29 ]
logger . info (
f " L_wr=[{l_wr[0]:+.2f},{l_wr[1]:+.2f},{l_wr[2]:+.2f}] "
f " R_wr=[{r_wr[0]:+.2f},{r_wr[1]:+.2f},{r_wr[2]:+.2f}] " )
if args . verbose and tracking and has_body_joints : if args . verbose and tracking and has_body_joints :
logger . debug ( logger . debug (
f " Full upper body: {np.array2string(current_angles[12:29], precision=3, separator= ' , ' )} " ) f " Full upper body: {np.array2string(current_angles[12:29], precision=3, separator= ' , ' )} " )
melancholytron
3 weeks ago