Godot calibration system from raw XRNode3D to calibrated bone offset / IK target. (MIT License)

calibrated_tracker.gd

# Copyright 2024 V-Sekai contributors
# SPDX-License-Identifier: MIT
@tool
extends Node3D

#@export_node_path("Node3D") var raw_tracker: NodePath:
	#set(value):
		#raw_tracker = value
		#raw_tracker_node = get_node_or_null(raw_tracker)
@export var raw_tracker_node: Node3D
@export_custom(PROPERTY_HINT_RANGE,"-3,3,0.01") var position_offset: Vector3
@export_custom(PROPERTY_HINT_RANGE,"-180,180,0.01,degrees") var rotation_euler_offset: Vector3

# Called when the node enters the scene tree for the first time.
#func _ready() -> void:
#	raw_tracker_node = get_node_or_null(raw_tracker)


# Called every frame. 'delta' is the elapsed time since the previous frame.
func _process(_delta: float) -> void:
	if raw_tracker_node == null:
		visible = false
		return
	#var parent := get_parent_node_3d()
	#if parent == null:
		#return
	var quat_offset: Quaternion = Quaternion.from_euler(rotation_euler_offset)
	#print(raw_tracker_node.global_position)
	#print(position_offset)
	if not basis.is_finite():
		basis = Basis.IDENTITY
	#print(quat_offset)
	#print(raw_tracker_node.global_transform.basis.get_rotation_quaternion())
	#print(raw_tracker_node.global_rotation * position_offset)
	#print(raw_tracker_node.global_position + raw_tracker_node.global_transform * position_offset)
	var raw_tracker_transform := Transform3D.IDENTITY
	if raw_tracker_node.get_parent_node_3d() != null:
		raw_tracker_transform = raw_tracker_node.get_parent_node_3d().global_transform
	raw_tracker_transform *= raw_tracker_node.transform.orthonormalized()
	position = get_parent().global_transform.affine_inverse() * (raw_tracker_transform * position_offset)
	#print((raw_tracker_node.global_transform.basis.get_rotation_quaternion() * quat_offset).get_euler())
	global_transform = Transform3D(Basis(raw_tracker_node.global_transform.basis.get_rotation_quaternion() * quat_offset), position)

calibration_orchestrator.gd

# Copyright 2024 V-Sekai contributors
# SPDX-License-Identifier: MIT
@tool
extends Node3D

const calibrated_tracker_script := preload("./calibrated_tracker.gd")

@export var skel: Skeleton3D

@export var calibration: bool
@export var calibrate_once: bool

@export var raw_tracker_root: Node3D
@export var raw_trackers: Array[Node3D]

var trackers_by_name: Dictionary
var trackers_to_name: Dictionary

const MAX_CALIBRATION_PAIR_DISTANCE := 1.234

signal tracker_disabled(tracker_node: calibrated_tracker_script)
signal tracker_changed(tracker_node: calibrated_tracker_script)
signal tracker_enabled(tracker_node: calibrated_tracker_script)


# Called when the node enters the scene tree for the first time.
func _ready() -> void:
	pass # Replace with function body.


func find_raw_trackers() -> Dictionary:
	if skel == null:
		return {}
	if raw_tracker_root != null:
		var matched = 0
		for n in raw_trackers:
			if n.get_parent() == raw_tracker_root:
				matched += 1
		if matched != raw_tracker_root.get_child_count():
			raw_trackers.clear()
			for n in raw_tracker_root.get_children():
				if n is Node3D:
					raw_trackers.append(n)
	trackers_by_name.clear()
	trackers_to_name.clear()
	for n in raw_trackers:
		trackers_by_name[n.name] = n
		trackers_to_name[n] = n.name

	# exclude hands and head from whitelist for now. those are automatic.
	var connection_bones := PackedStringArray(["LeftFoot", "RightFoot", "Hips"])
	var connection_points: PackedVector3Array

	var connected_trackers: Dictionary # tracker -> bone
	var connected_tracked_bones: Dictionary # bone -> tracker

	for bone_name in connection_bones:
		connection_points.append(skel.global_transform * skel.get_bone_global_pose(skel.find_bone(bone_name)).origin)
	#print("find cal " + str(connection_points))

	for tracker in raw_trackers:
		var builtin_bone_name: String = ""
		var xr_node = tracker as XRNode3D
		if xr_node != null:
			if xr_node.pose != &"default": # FIXME: Which pose is correct for hand position?
				continue
			match xr_node.tracker:
				&"left_hand":
					builtin_bone_name = "LeftHand"
				&"right_hand":
					builtin_bone_name = "RightHand"
				&"head":
					builtin_bone_name = "Head"
		var xr_camera = tracker as XRCamera3D
		if xr_camera:
			builtin_bone_name = "Head"
		if not builtin_bone_name.is_empty() and connected_tracked_bones.get(builtin_bone_name) == null:
			connected_trackers[tracker] = builtin_bone_name
			connected_tracked_bones[builtin_bone_name] = tracker

	# minimize distance(bone, tracker) for all combinations of bone, tracker.
	# then, apply the delta transform.
	#print(raw_trackers)
	#print(connected_trackers)
	for i in range(len(connection_points)):
		#print(connection_points)
		var min_dist_sq: float = -1.0
		var min_tracker: Node3D = null
		var min_bone_idx: int = -1
		for tracker in raw_trackers:
			if connected_trackers.has(tracker):
				continue
			var pos = tracker.global_position
			for j in range(len(connection_points)):
				if connected_tracked_bones.has(connection_bones[j]):
					continue
				var point: Vector3 = connection_points[j]
				if point.distance_squared_to(pos) < min_dist_sq or min_dist_sq < 0:
					min_dist_sq = point.distance_squared_to(pos)
					min_tracker = tracker
					min_bone_idx = j
					#print("Found a bone " + str(connection_bones[min_bone_idx]) + " " + str(min_tracker) + " dist " + str(min_dist_sq))
		# Arbitrary threshold
		if min_dist_sq > MAX_CALIBRATION_PAIR_DISTANCE:
			#print("Min Distance sq = " + str(min_dist_sq))
			# TODO: Make 1 meter maximum distance customizable.
			continue
		if min_bone_idx < 0:
			#print("Break because no min_bone")
			break
		var min_bone: String = connection_bones[min_bone_idx]
		connected_trackers[min_tracker] = min_bone
		connected_tracked_bones[min_bone] = min_tracker

	return connected_tracked_bones

func calibrate() -> void:
	# Which bones are allowed? Answer: for now, whitelist 6pt in find_raw_trackers()
	# Calculate which trackers map to which skeleton points
	var connected_tracked_bones: Dictionary = find_raw_trackers() # bone -> tracker
	if connected_tracked_bones.is_empty():
		return

	# Then, map those to existing trackers (found / disconnected trackers)
	# Then, disable (did not delete) any found / disconnected that didn't match
	var existing_tracker_nodes_by_name: Dictionary
	for chld in get_children():
		var calibrated_tracker := chld as calibrated_tracker_script
		if calibrated_tracker and calibrated_tracker.visible and not connected_tracked_bones.has(calibrated_tracker.name):
			print("calibration_orchestrator: Disable child " + str(chld.name) + " " + str(calibrated_tracker))
			calibrated_tracker.visible = false
			tracker_disabled.emit(calibrated_tracker)
		elif connected_tracked_bones.has(chld.name):
			if calibrated_tracker:
				existing_tracker_nodes_by_name[chld.name] = calibrated_tracker
				calibrated_tracker.raw_tracker_node = null
				if not calibrated_tracker.visible:
					print("calibration_orchestrator: Existing child " + str(chld.name) + " " + str(calibrated_tracker))
					#print(connected_tracked_bones)
					#print(existing_tracker_nodes_by_name)
					calibrated_tracker.visible = true
			else:
				if not chld.name.begins_with("_"):
					chld.name = "_" + chld.name

	# Finally, add any new trackers we discovered.
	for tracked_bone in connected_tracked_bones:
		if not existing_tracker_nodes_by_name.has(tracked_bone):
			print(existing_tracker_nodes_by_name)
			# Add any new trackers.
			var chld3d := Marker3D.new()
			# Child node names should be based on the calibrated bone name (LeftFoot, Head)
			# (not the source tracker name such as VIVE_1234 or LeftController)
			chld3d.name = tracked_bone
			chld3d.set_script(calibrated_tracker_script)
			# TODO: Set default position?
			add_child(chld3d)
			chld3d.owner = self if owner == null else owner
			print("calibration_orchestrator: Add child " + str(tracked_bone) + " " + str(chld3d))
			#print(connected_tracked_bones)
			#print(existing_tracker_nodes_by_name)
			existing_tracker_nodes_by_name[tracked_bone] = chld3d
		var calibrated_tracker := existing_tracker_nodes_by_name[tracked_bone] as calibrated_tracker_script
		if calibrated_tracker.raw_tracker_node != connected_tracked_bones[tracked_bone]:
			var was_null: bool = calibrated_tracker.raw_tracker_node == null
			calibrated_tracker.raw_tracker_node = connected_tracked_bones[tracked_bone]
			if was_null:
				tracker_enabled.emit(calibrated_tracker)
			else:
				tracker_changed.emit(calibrated_tracker)


	for bone_name in existing_tracker_nodes_by_name:
		var calibrated_tracker := existing_tracker_nodes_by_name[bone_name] as calibrated_tracker_script
		if calibrated_tracker == null:
			continue
		var raw_tracker_node: Node3D = calibrated_tracker.raw_tracker_node
		if raw_tracker_node == null:
			continue

		var skel_bone_position: Transform3D = (skel.global_transform * skel.get_bone_global_pose(skel.find_bone(bone_name)))

		var raw_tracker_transform := Transform3D.IDENTITY
		if raw_tracker_node.get_parent_node_3d() != null:
			raw_tracker_transform = raw_tracker_node.get_parent_node_3d().global_transform
		raw_tracker_transform *= raw_tracker_node.transform.orthonormalized()
		#print("calibration_orchestrator: tracker " + str(bone_name) + " pos " + str(raw_tracker_node.global_transform) + " and " + str(skel_bone_position.origin))
		calibrated_tracker.position_offset = raw_tracker_transform.affine_inverse() * skel_bone_position.origin
		#print("calibration_orchestrator: tracker " + str(bone_name) + " rot " + str(raw_tracker_node.global_transform.basis.get_rotation_quaternion()) + " and " + str(skel_bone_position.basis.get_rotation_quaternion()))
		var quat_offset: Quaternion = raw_tracker_node.global_transform.basis.get_rotation_quaternion().inverse() * skel_bone_position.basis.get_rotation_quaternion()
		calibrated_tracker.rotation_euler_offset = quat_offset.get_euler()


# Called every frame. 'delta' is the elapsed time since the previous frame.
func _process(delta: float) -> void:
	if calibration or calibrate_once:
		calibrate()
	if calibrate_once:
		calibrate_once = false

copy_bones_modifier.gd

# Copyright 2024 V-Sekai contributors
# SPDX-License-Identifier: MIT
@tool
class_name CopyBonesModifier3D
extends SkeletonModifier3D

@export var source_skeleton: Skeleton3D

# Called every frame. 'delta' is the elapsed time since the previous frame.
func _process_modification() -> void:
	var skel := get_skeleton()
	if source_skeleton == null:
		return
	var hips_position: Vector3 = source_skeleton.get_bone_global_pose(source_skeleton.find_bone(&"Hips")).origin * skel.motion_scale / source_skeleton.motion_scale
	skel.set_bone_pose_position(skel.find_bone(&"Hips"), hips_position)
	for b in range(source_skeleton.get_bone_count()):
		var tgt_b: int = skel.find_bone(source_skeleton.get_bone_name(b))
		if tgt_b != -1:
			skel.set_bone_pose_rotation(tgt_b, source_skeleton.get_bone_pose_rotation(b))

#func _process(_delta: float):
	#print("Do process")
	#get_skeleton().clear_bones_global_pose_override()

human_reference_rig.gd

# Copyright 2024 V-Sekai contributors
# SPDX-License-Identifier: MIT
@tool
extends Skeleton3D

# TODO
# @export var armspan: float: set: recalculate_skeleton()
# @export var heaad_root_height: float: set: recalculate_skeleton()

const GodotPositionOffsets := {
	"Hips": Vector3(0, 1, 0),
	"LeftUpperLeg": Vector3(0.078713, -0.064749, -0.01534),
	"RightUpperLeg": Vector3(-0.078713, -0.064749, -0.01534),
	"LeftLowerLeg": Vector3(0, 0.42551, 0.003327),
	"RightLowerLeg": Vector3(0, 0.42551, 0.003327),
	"LeftFoot": Vector3(0, 0.426025, 0.029196),
	"RightFoot": Vector3(0, 0.426025, 0.029196),
	"Spine": Vector3(0, 0.097642, 0.001261),
	"Chest": Vector3(0, 0.096701, -0.009598),
	"Neck": Vector3(0, 0.159882, -0.02413),
	"Head": Vector3(0, 0.092236, 0.016159),
	"LeftShoulder": Vector3(0.043831, 0.104972, -0.025203),
	"RightShoulder": Vector3(-0.043826, 0.104974, -0.025203),
	"LeftUpperArm": Vector3(-0.021406, 0.101581, -0.005031),
	"RightUpperArm": Vector3(0.021406, 0.101586, -0.005033),
	"LeftLowerArm": Vector3(0, 0.267001, 0),
	"RightLowerArm": Vector3(0, 0.267001, 0),
	"LeftHand": Vector3(0, 0.271675, 0),
	"RightHand": Vector3(0, 0.271675, 0),
	"LeftToes": Vector3(0, 0.102715, -0.083708),
	"RightToes": Vector3(0, 0.102715, -0.083708),
}


func _init():
	clear_bones()
	var sp := SkeletonProfileHumanoid.new()
	for b in range(sp.bone_size):
		add_bone(sp.get_bone_name(b))
	for b in range(sp.bone_size):
		set_bone_parent(b, sp.find_bone(sp.get_bone_parent(b)))
		var t: Transform3D = sp.get_reference_pose(b)
		if GodotPositionOffsets.has(sp.get_bone_name(b)):
			t.origin = GodotPositionOffsets[sp.get_bone_name(b)]
		set_bone_rest(b, t)
		set_bone_pose_position(b, t.origin)
		set_bone_pose_rotation(b, t.basis.get_rotation_quaternion())
		#set_bone_pose_scale(b, t.basis.get_scale())