You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
355 lines
14 KiB
355 lines
14 KiB
extends Node3D
|
|
## Reads XR_FB_body_tracking joints each frame via Godot's XRBodyTracker.
|
|
## Computes chest-relative wrist positions and emits tracking data.
|
|
## Visualizes all body joints as colored spheres.
|
|
##
|
|
## Meta body tracking provides 70 joints (remapped to Godot enum). We use:
|
|
## CHEST (3) - torso orientation (solves body rotation problem)
|
|
## HEAD (6) - head pose
|
|
## LEFT_HAND (22) - left wrist
|
|
## RIGHT_HAND (49) - right wrist
|
|
## Hand joints (25-48, 52-75) - finger positions
|
|
##
|
|
## Output poses are in Godot's coordinate system (Y-up, -Z forward).
|
|
## The server handles conversion to robot conventions.
|
|
|
|
## Emitted every frame with tracking data dict ready to send.
|
|
signal tracking_data_ready(data: Dictionary)
|
|
|
|
## Joint indices from Godot's XRBodyTracker.Joint enum
|
|
## NOTE: These are NOT the raw Meta XR_FB_body_tracking indices!
|
|
## The Godot OpenXR vendors plugin remaps Meta indices to Godot's enum.
|
|
## Reference: https://docs.godotengine.org/en/stable/classes/class_xrbodytracker.html
|
|
const JOINT_ROOT := 0 # XRBodyTracker.JOINT_ROOT
|
|
const JOINT_HIPS := 1 # XRBodyTracker.JOINT_HIPS
|
|
const JOINT_SPINE_LOWER := 2 # XRBodyTracker.JOINT_SPINE
|
|
const JOINT_CHEST := 3 # XRBodyTracker.JOINT_CHEST
|
|
const JOINT_UPPER_CHEST := 4 # XRBodyTracker.JOINT_UPPER_CHEST
|
|
const JOINT_NECK := 5 # XRBodyTracker.JOINT_NECK
|
|
const JOINT_HEAD := 6 # XRBodyTracker.JOINT_HEAD
|
|
|
|
const JOINT_LEFT_SHOULDER := 8 # XRBodyTracker.JOINT_LEFT_SHOULDER
|
|
const JOINT_LEFT_ARM_UPPER := 9 # XRBodyTracker.JOINT_LEFT_UPPER_ARM
|
|
const JOINT_LEFT_ARM_LOWER := 10 # XRBodyTracker.JOINT_LEFT_LOWER_ARM
|
|
const JOINT_LEFT_HAND_WRIST := 22 # XRBodyTracker.JOINT_LEFT_HAND
|
|
|
|
const JOINT_RIGHT_SHOULDER := 11 # XRBodyTracker.JOINT_RIGHT_SHOULDER
|
|
const JOINT_RIGHT_ARM_UPPER := 12 # XRBodyTracker.JOINT_RIGHT_UPPER_ARM
|
|
const JOINT_RIGHT_ARM_LOWER := 13 # XRBodyTracker.JOINT_RIGHT_LOWER_ARM
|
|
const JOINT_RIGHT_HAND_WRIST := 49 # XRBodyTracker.JOINT_RIGHT_HAND
|
|
|
|
# Hand joints: Godot left hand fingers start at 25, right at 52
|
|
# (25 joints per hand, same layout as XR_EXT_hand_tracking)
|
|
const JOINT_LEFT_HAND_START := 25
|
|
const JOINT_RIGHT_HAND_START := 52
|
|
const HAND_JOINT_COUNT := 25
|
|
|
|
## Total body joint count (Godot XRBodyTracker.JOINT_MAX = 87)
|
|
const BODY_JOINT_COUNT := 87
|
|
|
|
## Joint visualization colors
|
|
const COLOR_BODY := Color(1.0, 0.7, 0.2, 1.0) # Orange - spine/torso
|
|
const COLOR_HEAD := Color(1.0, 1.0, 1.0, 1.0) # White
|
|
const COLOR_LEFT_ARM := Color(0.3, 0.5, 1.0, 1.0) # Blue
|
|
const COLOR_RIGHT_ARM := Color(0.3, 1.0, 0.5, 1.0) # Green
|
|
const COLOR_LEFT_HAND := Color(0.5, 0.7, 1.0, 1.0) # Light blue
|
|
const COLOR_RIGHT_HAND := Color(0.5, 1.0, 0.7, 1.0) # Light green
|
|
|
|
## Tracking state
|
|
var body_tracker_name: StringName = &"/user/body_tracker"
|
|
var is_tracking: bool = false
|
|
var frames_since_last_send: int = 0
|
|
|
|
## Send rate control: target ~30 Hz (every other frame at 72 Hz)
|
|
@export var send_every_n_frames: int = 2
|
|
|
|
## Debug logging
|
|
@export var debug_log: bool = false
|
|
var _log_counter: int = 0
|
|
|
|
## Joint visualization
|
|
var _xr_origin: XROrigin3D
|
|
var _joint_spheres: Array = [] # Array of MeshInstance3D, indexed by joint
|
|
|
|
|
|
func setup(xr_origin: XROrigin3D) -> void:
|
|
_xr_origin = xr_origin
|
|
_create_joint_spheres()
|
|
|
|
|
|
func _ready() -> void:
|
|
print("[BodyTracker] Initialized, waiting for body tracking data...")
|
|
|
|
|
|
func _create_joint_spheres() -> void:
|
|
var body_mesh := SphereMesh.new()
|
|
body_mesh.radius = 0.025
|
|
body_mesh.height = 0.05
|
|
|
|
# Only create spheres for body/arm joints (0-21), skip hand joints (22+)
|
|
# Hand joints are already visualized by vr_ui_pointer.gd
|
|
_joint_spheres.resize(BODY_JOINT_COUNT)
|
|
for i in range(BODY_JOINT_COUNT):
|
|
if i >= JOINT_LEFT_HAND_WRIST: # 22+ = hand/finger joints
|
|
_joint_spheres[i] = null
|
|
continue
|
|
var s := MeshInstance3D.new()
|
|
s.mesh = body_mesh
|
|
var mat := StandardMaterial3D.new()
|
|
mat.shading_mode = BaseMaterial3D.SHADING_MODE_UNSHADED
|
|
mat.albedo_color = _get_joint_color(i)
|
|
s.material_override = mat
|
|
s.visible = false
|
|
add_child(s)
|
|
_joint_spheres[i] = s
|
|
|
|
|
|
func _get_joint_color(joint_idx: int) -> Color:
|
|
if joint_idx == JOINT_HEAD: # 6
|
|
return COLOR_HEAD
|
|
elif joint_idx <= JOINT_NECK: # 0-5 = body/spine
|
|
return COLOR_BODY
|
|
elif joint_idx >= JOINT_LEFT_SHOULDER and joint_idx <= JOINT_LEFT_ARM_LOWER: # 8-10
|
|
return COLOR_LEFT_ARM
|
|
elif joint_idx >= JOINT_RIGHT_SHOULDER and joint_idx <= JOINT_RIGHT_ARM_LOWER: # 11-13
|
|
return COLOR_RIGHT_ARM
|
|
elif joint_idx >= 14 and joint_idx <= 21: # legs
|
|
return COLOR_BODY
|
|
elif joint_idx >= JOINT_LEFT_HAND_WRIST and joint_idx < JOINT_RIGHT_HAND_WRIST: # 22-48
|
|
return COLOR_LEFT_HAND
|
|
elif joint_idx >= JOINT_RIGHT_HAND_WRIST: # 49+
|
|
return COLOR_RIGHT_HAND
|
|
return COLOR_BODY
|
|
|
|
|
|
func _process(_delta: float) -> void:
|
|
frames_since_last_send += 1
|
|
if frames_since_last_send < send_every_n_frames:
|
|
return
|
|
frames_since_last_send = 0
|
|
|
|
var tracker := XRServer.get_tracker(body_tracker_name) as XRBodyTracker
|
|
if tracker == null:
|
|
if is_tracking:
|
|
print("[BodyTracker] Lost body tracking")
|
|
is_tracking = false
|
|
_hide_all_spheres()
|
|
return
|
|
|
|
if not tracker.get_has_tracking_data():
|
|
if is_tracking:
|
|
print("[BodyTracker] Body tracking data unavailable")
|
|
is_tracking = false
|
|
_hide_all_spheres()
|
|
return
|
|
|
|
if not is_tracking:
|
|
print("[BodyTracker] Body tracking active!")
|
|
is_tracking = true
|
|
|
|
# Update joint sphere positions
|
|
_update_joint_spheres(tracker)
|
|
|
|
# Read key joint poses
|
|
var chest_xform := tracker.get_joint_transform(JOINT_CHEST)
|
|
var head_xform := tracker.get_joint_transform(JOINT_HEAD)
|
|
var left_wrist_xform := tracker.get_joint_transform(JOINT_LEFT_HAND_WRIST)
|
|
var right_wrist_xform := tracker.get_joint_transform(JOINT_RIGHT_HAND_WRIST)
|
|
|
|
# Compute chest-relative wrist positions
|
|
# This is the key advantage: eliminates head-position subtraction artifacts
|
|
var chest_inv := chest_xform.affine_inverse()
|
|
var left_wrist_rel := chest_inv * left_wrist_xform
|
|
var right_wrist_rel := chest_inv * right_wrist_xform
|
|
|
|
# Build tracking data packet
|
|
var data := {}
|
|
data["type"] = "tracking"
|
|
data["timestamp"] = Time.get_ticks_msec()
|
|
|
|
# Head pose (world space) - 7 floats: pos(3) + quat(4)
|
|
data["head"] = _xform_to_pose7(head_xform)
|
|
|
|
# Chest pose (world space) - 7 floats: pos(3) + quat(4)
|
|
data["chest"] = _xform_to_pose7(chest_xform)
|
|
|
|
# Chest-relative wrist poses as 4x4 matrices (16 floats each, column-major)
|
|
# These replace the world-space wrist poses; no head subtraction needed
|
|
data["left_wrist"] = _xform_to_mat16(left_wrist_rel)
|
|
data["right_wrist"] = _xform_to_mat16(right_wrist_rel)
|
|
|
|
# Get XRHandTracker references (used for finger positions AND wrist rotation)
|
|
var left_ht = XRServer.get_tracker(&"/user/hand_tracker/left") as XRHandTracker
|
|
var right_ht = XRServer.get_tracker(&"/user/hand_tracker/right") as XRHandTracker
|
|
|
|
# Hand joint positions (25 joints per hand, relative to wrist)
|
|
# Use XRHandTracker for finger articulation — XRBodyTracker doesn't provide it on Quest 3
|
|
var left_hand_pos: Array
|
|
var right_hand_pos: Array
|
|
if left_ht and left_ht.has_tracking_data:
|
|
left_hand_pos = _get_hand_positions_ht(left_ht)
|
|
else:
|
|
left_hand_pos = _get_hand_positions(tracker, JOINT_LEFT_HAND_START, left_wrist_xform)
|
|
if right_ht and right_ht.has_tracking_data:
|
|
right_hand_pos = _get_hand_positions_ht(right_ht)
|
|
else:
|
|
right_hand_pos = _get_hand_positions(tracker, JOINT_RIGHT_HAND_START, right_wrist_xform)
|
|
data["left_hand_pos"] = left_hand_pos
|
|
data["right_hand_pos"] = right_hand_pos
|
|
|
|
# Hand joint rotations (25 joints per hand, relative to wrist)
|
|
var left_hand_rot := _get_hand_rotations(tracker, JOINT_LEFT_HAND_START, left_wrist_xform)
|
|
var right_hand_rot := _get_hand_rotations(tracker, JOINT_RIGHT_HAND_START, right_wrist_xform)
|
|
data["left_hand_rot"] = left_hand_rot
|
|
data["right_hand_rot"] = right_hand_rot
|
|
|
|
# Body joint transforms for retargeting (world-space pose7)
|
|
# Order: hips, chest, L_upper_arm, L_lower_arm, L_wrist, R_upper_arm, R_lower_arm, R_wrist
|
|
# Each joint: [x, y, z, qx, qy, qz, qw] = 7 floats, total 56 floats
|
|
var hips_xform := tracker.get_joint_transform(JOINT_HIPS)
|
|
var left_upper_arm_xform := tracker.get_joint_transform(JOINT_LEFT_ARM_UPPER)
|
|
var left_lower_arm_xform := tracker.get_joint_transform(JOINT_LEFT_ARM_LOWER)
|
|
var right_upper_arm_xform := tracker.get_joint_transform(JOINT_RIGHT_ARM_UPPER)
|
|
var right_lower_arm_xform := tracker.get_joint_transform(JOINT_RIGHT_ARM_LOWER)
|
|
|
|
# Use XRHandTracker wrist transforms instead of body tracker wrist —
|
|
# hand tracking has much better wrist rotation detection
|
|
var left_wrist_body := left_wrist_xform
|
|
var right_wrist_body := right_wrist_xform
|
|
if left_ht and left_ht.has_tracking_data:
|
|
var ht_wrist := left_ht.get_hand_joint_transform(XRHandTracker.HAND_JOINT_WRIST)
|
|
if ht_wrist.origin != Vector3.ZERO:
|
|
left_wrist_body = ht_wrist
|
|
if right_ht and right_ht.has_tracking_data:
|
|
var ht_wrist := right_ht.get_hand_joint_transform(XRHandTracker.HAND_JOINT_WRIST)
|
|
if ht_wrist.origin != Vector3.ZERO:
|
|
right_wrist_body = ht_wrist
|
|
|
|
var body_joints := []
|
|
body_joints.append_array(_xform_to_pose7(hips_xform))
|
|
body_joints.append_array(_xform_to_pose7(chest_xform))
|
|
body_joints.append_array(_xform_to_pose7(left_upper_arm_xform))
|
|
body_joints.append_array(_xform_to_pose7(left_lower_arm_xform))
|
|
body_joints.append_array(_xform_to_pose7(left_wrist_body))
|
|
body_joints.append_array(_xform_to_pose7(right_upper_arm_xform))
|
|
body_joints.append_array(_xform_to_pose7(right_lower_arm_xform))
|
|
body_joints.append_array(_xform_to_pose7(right_wrist_body))
|
|
data["body_joints"] = body_joints
|
|
|
|
# Debug logging every ~2 seconds
|
|
if debug_log:
|
|
_log_counter += 1
|
|
if _log_counter >= 60:
|
|
_log_counter = 0
|
|
print("[BodyTracker] chest=", chest_xform.origin,
|
|
" head=", head_xform.origin,
|
|
" L_wrist_rel=", left_wrist_rel.origin,
|
|
" R_wrist_rel=", right_wrist_rel.origin)
|
|
|
|
tracking_data_ready.emit(data)
|
|
|
|
|
|
## Convert Transform3D to 7-float pose: [x, y, z, qx, qy, qz, qw]
|
|
func _xform_to_pose7(xform: Transform3D) -> Array:
|
|
var pos := xform.origin
|
|
var quat := xform.basis.get_rotation_quaternion()
|
|
return [pos.x, pos.y, pos.z, quat.x, quat.y, quat.z, quat.w]
|
|
|
|
|
|
## Convert Transform3D to 16-float column-major 4x4 matrix
|
|
## Layout: [m00, m10, m20, 0, m01, m11, m21, 0, m02, m12, m22, 0, tx, ty, tz, 1]
|
|
## This matches NumPy's Fortran ('F') order used by the existing teleop code.
|
|
func _xform_to_mat16(xform: Transform3D) -> Array:
|
|
var b := xform.basis
|
|
var o := xform.origin
|
|
# Godot Basis: b[col][row] — but we store column-major
|
|
# Column 0: basis x-axis
|
|
# Column 1: basis y-axis
|
|
# Column 2: basis z-axis
|
|
# Column 3: origin (translation)
|
|
return [
|
|
b.x.x, b.x.y, b.x.z, 0.0, # Column 0
|
|
b.y.x, b.y.y, b.y.z, 0.0, # Column 1
|
|
b.z.x, b.z.y, b.z.z, 0.0, # Column 2
|
|
o.x, o.y, o.z, 1.0 # Column 3
|
|
]
|
|
|
|
|
|
## Get 25 hand joint positions from XRHandTracker, relative to wrist (75 floats).
|
|
## Joint order: PALM(0), THUMB_META..TIP(2-5), INDEX(6-10), MIDDLE(11-15),
|
|
## RING(16-20), LITTLE(21-25) — skipping WRIST(1).
|
|
## This matches the format expected by dex-retargeting / xr_teleoperate.
|
|
func _get_hand_positions_ht(hand_tracker: XRHandTracker) -> Array:
|
|
var wrist_xform := hand_tracker.get_hand_joint_transform(XRHandTracker.HAND_JOINT_WRIST)
|
|
var wrist_inv := wrist_xform.affine_inverse()
|
|
var positions := []
|
|
positions.resize(HAND_JOINT_COUNT * 3) # 25 * 3 = 75
|
|
var out_idx := 0
|
|
for xr_joint in range(26): # OpenXR joints 0-25
|
|
if xr_joint == 1: # Skip WRIST
|
|
continue
|
|
var joint_xform := hand_tracker.get_hand_joint_transform(xr_joint)
|
|
var rel := wrist_inv * joint_xform
|
|
positions[out_idx * 3 + 0] = rel.origin.x
|
|
positions[out_idx * 3 + 1] = rel.origin.y
|
|
positions[out_idx * 3 + 2] = rel.origin.z
|
|
out_idx += 1
|
|
return positions
|
|
|
|
|
|
## Get 25 hand joint positions from XRBodyTracker (fallback), relative to wrist.
|
|
## Each joint: [x, y, z] relative to wrist
|
|
func _get_hand_positions(tracker: XRBodyTracker, start_idx: int, wrist_xform: Transform3D) -> Array:
|
|
var wrist_inv := wrist_xform.affine_inverse()
|
|
var positions := []
|
|
positions.resize(HAND_JOINT_COUNT * 3)
|
|
for i in range(HAND_JOINT_COUNT):
|
|
var joint_xform := tracker.get_joint_transform(start_idx + i)
|
|
var rel := wrist_inv * joint_xform
|
|
positions[i * 3 + 0] = rel.origin.x
|
|
positions[i * 3 + 1] = rel.origin.y
|
|
positions[i * 3 + 2] = rel.origin.z
|
|
return positions
|
|
|
|
|
|
func _update_joint_spheres(tracker: XRBodyTracker) -> void:
|
|
if _joint_spheres.is_empty() or _xr_origin == null:
|
|
return
|
|
var origin_xform := _xr_origin.global_transform
|
|
for i in range(BODY_JOINT_COUNT):
|
|
if _joint_spheres[i] == null:
|
|
continue
|
|
var xform := tracker.get_joint_transform(i)
|
|
if xform.origin == Vector3.ZERO:
|
|
_joint_spheres[i].visible = false
|
|
continue
|
|
_joint_spheres[i].global_position = origin_xform * xform.origin
|
|
_joint_spheres[i].visible = true
|
|
|
|
|
|
func _hide_all_spheres() -> void:
|
|
for s in _joint_spheres:
|
|
if s != null:
|
|
s.visible = false
|
|
|
|
|
|
## Get 25 hand joint rotations relative to wrist, as flat array (225 floats)
|
|
## Each joint: 9 floats (3x3 rotation matrix, column-major)
|
|
func _get_hand_rotations(tracker: XRBodyTracker, start_idx: int, wrist_xform: Transform3D) -> Array:
|
|
var wrist_rot_inv := wrist_xform.basis.inverse()
|
|
var rotations := []
|
|
rotations.resize(HAND_JOINT_COUNT * 9)
|
|
for i in range(HAND_JOINT_COUNT):
|
|
var joint_xform := tracker.get_joint_transform(start_idx + i)
|
|
var rel_basis := wrist_rot_inv * joint_xform.basis
|
|
# Store as column-major 3x3
|
|
rotations[i * 9 + 0] = rel_basis.x.x
|
|
rotations[i * 9 + 1] = rel_basis.x.y
|
|
rotations[i * 9 + 2] = rel_basis.x.z
|
|
rotations[i * 9 + 3] = rel_basis.y.x
|
|
rotations[i * 9 + 4] = rel_basis.y.y
|
|
rotations[i * 9 + 5] = rel_basis.y.z
|
|
rotations[i * 9 + 6] = rel_basis.z.x
|
|
rotations[i * 9 + 7] = rel_basis.z.y
|
|
rotations[i * 9 + 8] = rel_basis.z.z
|
|
return rotations
|