Godot Version
4.5.1
Question
The solution I have in VR seems to have serious problems. The Inverse Kinematics (IK) calculation is overly complex. I’m need help.
I used AI to help me write a script that implements player motion tracking using an IK (Inverse Kinematics) algorithm. However, the result is very poor; the movement of the two hands is completely chaotic. I’m confused by the complex calculations involved. Could you help me find the errors in the script?
Or could you recommend a robust/proven solution for me?
my code:
—————————————————————————————————
@tool
class_name VrAvatarModifier
extends SkeletonModifier3D
##==============================================================================
##Configuration Parameters
##==============================================================================
@export_group(“VR Tracking Nodes”)
@export var headGroupName: String = “vr_head”## Head controller group name
@export var leftHandGroupName: String = “vr_leftHand”## Left hand controller group name
@export var rightHandGroupName: String = “vr_rightHand”## Right hand controller group name
@export_group(“Body Rotation”)
@export var eyeToHeadOffset: Vector3 = Vector3(0.0, 0.0, 0.08)## Defines the offset from the eye to the skull center (backwards and down). Positive Z is usually backward (depends on your model orientation; in Godot -Z is forward). Negative Y is downward.
@export_range(0.0, 180.0) var bodyTurnThreshold: float = 45.0 ## Body follows when head turns beyond this angle
@export_range(0.0, 1.0) var bodyTurnSmoothing: float = 0.15 ## Body turn smoothing
@export_group(“Hand Settings”)
@export var handRotationOffsetY: float = 180.0 ## Palm Y-axis offset
@export var handRotationOffsetX: float = 0.0 ## Palm X-axis offset
@export var handRotationOffsetZ: float = 0.0 ## Palm Z-axis offset
@export_group(“Finger Settings”)
@export var fingerBendAxis: Vector3 = Vector3(1, 0, 0) ## Finger bend axis (X axis)
@export_range(0.0, 1.0) var fingerCurlStrength: float = 1.0
@export_group(“Leg IK - Locomotion”)
@export var enableLegIK: bool = true
@export var walkSpeedThreshold: float = 0.5 ## m/s, walking animation triggers above this speed
@export var stepHeight: float = 0.15 ## Step lift height
@export var stepLength: float = 0.4 ## Step length
@export var stairClimbHeightThreshold: float = 0.2 ## Trigger stair climbing when height change exceeds this value
@export_group(“Debug”)
@export var debugMode: bool = false
@export var showLocomotionDebug: bool = false
##==============================================================================
##Bone names (based on your MD file)
##==============================================================================
var bone_names = {
“hips”: “Hips”,
“spine”: “Spine”,
“spine1”: “Spine1”,
“spine2”: “Spine2”,
“neck”: “Neck”,
“head”: “Head”,
"left_shoulder": "LeftShoulder",
"left_arm": "LeftArm",
"left_forearm": "LeftForeArm",
"left_hand": "LeftHand",
"right_shoulder": "RightShoulder",
"right_arm": "RightArm",
"right_forearm": "RightForeArm",
"right_hand": "RightHand",
"left_upleg": "LeftUpLeg",
"left_leg": "LeftLeg",
"left_foot": "LeftFoot",
"left_toe": "LeftToeBase",
"right_upleg": "RightUpLeg",
"right_leg": "RightLeg",
"right_foot": "RightFoot",
"right_toe": "RightToeBase"
}
##==============================================================================
##Runtime variables
##==============================================================================
var xrCameraNode: XRCamera3D
var xrControllerLeftNode: XRController3D
var xrControllerRightNode: XRController3D
var initialized: bool = false
##Body bone index cache
var boneIdx: Dictionary = {}
##Body bone length cache
var boneLengths: Dictionary = {}
##Finger bone index cache
var finger_bones: Dictionary = {
“Left”: {“Thumb”:
, “Index”:
, “Middle”:
, “Ring”:
, “Pinky”:
},
“Right”: {“Thumb”:
, “Index”:
, “Middle”:
, “Ring”:
, “Pinky”:
}
}
##Controller inputs
var leftGrip: float = 0.0 ## Depth of grip trigger pressed
var leftTrigger: float = 0.0 ## Depth of side trigger pressed
var rightGrip: float = 0.0 ## Depth of grip trigger pressed
var rightTrigger: float = 0.0 ## Depth of side trigger pressed
##Body rotation state
var currentBodyYaw: float = 0.0 ## Current body Y-axis rotation angle (left-right rotation)
var targetBodyYaw: float = 0.0 ## Target body Y-axis rotation angle (left-right rotation)
##Locomotion tracking
var lastHmdPosition: Vector3 = Vector3.ZERO ## Current headset position, i.e., body position
var lastHmdHeight: float = 0.0 ## Current headset height, i.e., height value
var horizontalVelocity: float = 0.0
var verticalVelocity: float = 0.0
##Gait cycle
var walkCycleTime: float = 0.0
var stepFrequency: float = 2.0 ## Step frequency (steps/sec)
func _ready() → void:
if not Engine.is_editor_hint():
call_deferred(“Initialize”)
##Data initialization
func Initialize() → void:
# Wait for one physics frame to ensure node groups are loaded
await get_tree().process_frame
# Get the three control nodes from groups
var heads = get_tree().get_nodes_in_group(headGroupName)
if heads.size() > 0:
xrCameraNode = heads[0] as XRCamera3D
var l_hands = get_tree().get_nodes_in_group(leftHandGroupName)
if l_hands.size() > 0:
xrControllerLeftNode = l_hands[0] as XRController3D
var r_hands = get_tree().get_nodes_in_group(rightHandGroupName)
if r_hands.size() > 0:
xrControllerRightNode = r_hands[0] as XRController3D
initialized = (xrCameraNode != null and xrControllerLeftNode != null and xrControllerRightNode != null)
if initialized:
var currentSkeleton = get_skeleton()
if currentSkeleton:
CacheBones(currentSkeleton)
CalculateBoneLengths(currentSkeleton)
CacheFingerBones(currentSkeleton)
# Initialize positions
lastHmdPosition = xrCameraNode.global_position
lastHmdHeight = xrCameraNode.global_position.y
# Initialize body facing direction
var head_forward = -xrCameraNode.global_transform.basis.z
head_forward.y = 0
currentBodyYaw = atan2(head_forward.x, head_forward.z)
targetBodyYaw = currentBodyYaw
print("✓ VR Avatar initialization complete")
if debugMode:
print(" Bone count: ", currentSkeleton.get_bone_count())
print(" Core bone indices: ", boneIdx)
##Cache body bone indices
func CacheBones(skel: Skeleton3D) → void:
for key in bone_names.keys():
var idx = skel.find_bone(bone_names[key])
boneIdx[key] = idx
if idx == -1 and debugMode:
push_warning("Bone not found: " + bone_names[key])
##Calculate bone lengths
func CalculateBoneLengths(skel: Skeleton3D) → void:
# Arm lengths
var chains = [
[“left_arm”, “left_forearm”],
[“right_arm”, “right_forearm”],
[“left_upleg”, “left_leg”],
[“right_upleg”, “right_leg”]
]
for chain in chains:
var id_a = boneIdx[chain[0]]
var id_b = boneIdx[chain[1]]
if id_a != -1 and id_b != -1:
var rest_b = skel.get_bone_rest(id_b)
# Calculate bone length from rest pose
boneLengths[chain[0]] = rest_b.origin.length()
# Calculate child bone length
var children = skel.get_bone_children(id_b)
if children.size() > 0:
var rest_c = skel.get_bone_rest(children[0])
boneLengths[chain[1]] = rest_c.origin.length()
if debugMode:
print("Bone lengths: ", boneLengths)
##Cache finger bone indices
func CacheFingerBones(skel: Skeleton3D) → void:
var sides = [“Left”, “Right”]
var fingers = [“Thumb”, “Index”, “Middle”, “Ring”, “Pinky”]
for side in sides:
for finger in fingers:
var ids = []
for i in range(1, 5):
var bone_name = side + "Hand" + finger + str(i)
var idx = skel.find_bone(bone_name)
if idx != -1:
ids.append(idx)
finger_bones[side][finger] = ids
##==============================================================================
##Main loop
##==============================================================================
func _process_modification() → void:
# All skeleton control must be performed within this function
if not initialized or Engine.is_editor_hint():
return
var skel = get_skeleton()
if not skel:
return
# Update inputs and locomotion tracking variables
UpdateControllerInput()
UpdateLocomotionTracking(get_process_delta_time())
var skel_inv = skel.global_transform.affine_inverse()
# 1. Spine and head
ProcessSpineAndHead(skel, skel_inv)
# 2. Body core: Hips position + body rotation
ProcessBodyRotationAndPosition(skel, skel_inv)
# 3. Arm IK
ProcessArmIk(skel, skel_inv, true) # Left arm
ProcessArmIk(skel, skel_inv, false) # Right arm
# 4. Leg IK (walking + stairs)
#if enableLegIK:
#_process_leg_locomotion(skel, skel_inv, get_process_delta_time())
# 5. Finger animation
#_process_fingers(skel, true) # Left hand
#_process_fingers(skel, false) # Right hand
##Update controller input variables: depth of side trigger and grip pressed
func UpdateControllerInput() → void:
if xrControllerLeftNode:
leftTrigger = xrControllerLeftNode.get_float(“trigger”)
leftGrip = xrControllerLeftNode.get_float(“grip”)
if xrControllerRightNode:
rightTrigger = xrControllerRightNode.get_float(“trigger”)
rightGrip = xrControllerRightNode.get_float(“grip”)
##Update horizontal and vertical velocity, and gait cycle time
func UpdateLocomotionTracking(delta: float) → void:
if not xrCameraNode:
return
var current_pos = xrCameraNode.global_position
var current_height = current_pos.y
# Calculate horizontal velocity
var horizontal_delta = Vector3(
current_pos.x - lastHmdPosition.x,
0,
current_pos.z - lastHmdPosition.z
)
horizontalVelocity = horizontal_delta.length() / delta
# Calculate vertical velocity
verticalVelocity = (current_height - lastHmdHeight) / delta
# Update gait cycle time
if horizontalVelocity > walkSpeedThreshold:
walkCycleTime += delta * stepFrequency
else:
walkCycleTime = 0.0
lastHmdPosition = current_pos
lastHmdHeight = current_height
if showLocomotionDebug:
print("Horizontal speed: %.2f m/s | Vertical speed: %.2f m/s | Gait: %.2f" % [horizontalVelocity, verticalVelocity, walkCycleTime])
##Calculate and apply per-frame spine and head
func ProcessSpineAndHead(skel: Skeleton3D, skel_inv: Transform3D) → void:
# Keep the spine upright (optional: add slight bend)
var spine_indices = [“spine”, “spine1”, “spine2”]
for key in spine_indices:
var idx = boneIdx[key]
if idx != -1:
# Keep rest pose or slightly follow body rotation
pass
# Head tracking
var head_idx = boneIdx["head"]
if head_idx == -1:
return
# Calculate head target transform
var target_local = skel_inv * xrCameraNode.global_transform
var parent_idx = skel.get_bone_parent(head_idx)
var parent_global = skel.get_bone_global_pose(parent_idx)
# Calculate head rotation relative to parent bone
var head_basis = parent_global.basis.inverse() * target_local.basis
# Apply head rotation
skel.set_bone_pose_rotation(head_idx, Quaternion(head_basis))
##Calculate and apply per-frame body rotation and position
func ProcessBodyRotationAndPosition(skel: Skeleton3D, skel_inv: Transform3D) → void:
var hips_idx = boneIdx[“hips”]
if hips_idx == -1:
return
# Calculate head facing direction
var head_forward = xrCameraNode.global_transform.basis.z
head_forward.y = 0
if head_forward.length() < 0.001:
return
head_forward = head_forward.normalized()
var head_yaw = atan2(head_forward.x, head_forward.z)
# Calculate angle difference between head and body
var yaw_diff = AngleDifference(head_yaw, currentBodyYaw)
# If beyond threshold, body follows rotation
if abs(yaw_diff) > deg_to_rad(bodyTurnThreshold):
targetBodyYaw = head_yaw
# Smoothly interpolate body rotation
currentBodyYaw = lerp_angle(currentBodyYaw, targetBodyYaw, bodyTurnSmoothing)
# Build body facing basis
var body_forward = Vector3(sin(currentBodyYaw), 0, cos(currentBodyYaw))
var body_basis = Basis.looking_at(body_forward, Vector3.UP)
##1. Get camera global transform
var cam_global = xrCameraNode.global_transform
# 2. [New step] Apply offset
# Convert the local offset to global direction and add to the camera position
# The effect: camera does not move, but when we compute the body's target position, we consider the skull to be behind the camera
var head_bone_global_origin = cam_global * eyeToHeadOffset
# 3. Convert the computed "skull position" to skeleton local space
var head_local_pos = skel_inv * head_bone_global_origin
var hips_pose = skel.get_bone_pose(hips_idx)
# Hips Y height: use adjusted skull height - 0.7m
var target_hips_y = max(0.8, head_local_pos.y - 0.7)
# Hips horizontal position: use adjusted skull position
hips_pose.origin = Vector3(head_local_pos.x, target_hips_y, head_local_pos.z)
# Set Hips rotation
var hips_local_basis = skel.global_transform.basis.inverse() * body_basis
hips_pose.basis = hips_local_basis.orthonormalized()
skel.set_bone_pose(hips_idx, hips_pose)
##Calculate angle difference
func AngleDifference(a: float, b: float) → float:
var diff = a - b
while diff > PI:
diff -= TAU
while diff < -PI:
diff += TAU
return diff
##==============================================================================
##Arm IK
##==============================================================================
func ProcessArmIk(skel: Skeleton3D, skel_inv: Transform3D, is_left: bool) → void:
var prefix = “left_” if is_left else “right_”
var target_node = xrControllerLeftNode if is_left else xrControllerRightNode
var shoulder_idx = boneIdx[prefix + "shoulder"]
var upper_idx = boneIdx[prefix + "arm"]
var lower_idx = boneIdx[prefix + "forearm"]
var hand_idx = boneIdx[prefix + "hand"]
if upper_idx == -1 or lower_idx == -1 or hand_idx == -1:
return
# IK solving
var target_pos = target_node.global_position
var upper_len = boneLengths.get(prefix + "arm", 0.3)
var lower_len = boneLengths.get(prefix + "forearm", 0.25)
# Pole vector (elbow hint direction)
var pole_hint = Vector3.DOWN if is_left else Vector3.DOWN # Elbow points downward
_solve_two_bone_ik(skel, upper_idx, lower_idx, hand_idx, target_pos, upper_len, lower_len, pole_hint)
# Palm rotation
var target_local = skel_inv * target_node.global_transform
var parent_global = skel.get_bone_global_pose(skel.get_bone_parent(hand_idx))
var hand_basis = parent_global.basis.inverse() * target_local.basis
# Apply palm rotation offsets (because in A-Pose palms face the thighs)
hand_basis = hand_basis.rotated(Vector3.UP, deg_to_rad(handRotationOffsetY))
hand_basis = hand_basis.rotated(Vector3.RIGHT, deg_to_rad(handRotationOffsetX))
hand_basis = hand_basis.rotated(Vector3.BACK, deg_to_rad(handRotationOffsetZ))
skel.set_bone_pose_rotation(hand_idx, Quaternion(hand_basis))
func _solve_two_bone_ik(skel: Skeleton3D, id_upper: int, id_lower: int, id_end: int,
target_pos: Vector3, len_upper: float, len_lower: float,
pole_hint: Vector3) → void:
# Get root position (shoulder/hip)
var root_pos = skel.get_bone_global_pose(id_upper).origin
var to_target = target_pos - root_pos
var distance = to_target.length()
# Limit maximum distance
var max_reach = len_upper + len_lower - 0.01
if distance > max_reach:
distance = max_reach
to_target = to_target.normalized() * distance
# Use cosine law to compute joint angles
var cos_lower_angle = (len_upper * len_upper + len_lower * len_lower - distance * distance) / (2.0 * len_upper * len_lower)
cos_lower_angle = clamp(cos_lower_angle, -1.0, 1.0)
var lower_angle = PI - acos(cos_lower_angle)
var cos_upper_angle = (distance * distance + len_upper * len_upper - len_lower * len_lower) / (2.0 * distance * len_upper)
cos_upper_angle = clamp(cos_upper_angle, -1.0, 1.0)
var upper_angle = acos(cos_upper_angle)
# Compute bending plane
var forward = to_target.normalized()
var bend_axis = forward.cross(pole_hint).normalized()
if bend_axis.length_squared() < 0.01:
bend_axis = Vector3.RIGHT if abs(forward.y) < 0.9 else Vector3.FORWARD
# Upper arm rotation
var upper_rot = Basis.looking_at(forward, pole_hint).rotated(bend_axis, -upper_angle)
var parent_upper = skel.get_bone_parent(id_upper)
var parent_global_upper = Transform3D.IDENTITY if parent_upper == -1 else skel.get_bone_global_pose(parent_upper)
var upper_local = parent_global_upper.basis.inverse() * upper_rot
skel.set_bone_pose_rotation(id_upper, Quaternion(upper_local))
# Lower arm rotation (relative to upper arm)
var lower_rot = Quaternion(bend_axis, lower_angle)
skel.set_bone_pose_rotation(id_lower, lower_rot)
##==============================================================================
##Leg IK (walking + stairs)
##==============================================================================
func _process_leg_locomotion(skel: Skeleton3D, skel_inv: Transform3D, delta: float) → void:
var hips_global = skel.get_bone_global_pose(boneIdx[“hips”])
var hips_world_pos = skel.global_transform * hips_global.origin
# Determine if climbing stairs
var is_climbing = abs(verticalVelocity) > stairClimbHeightThreshold
# Determine if walking
var is_walking = horizontalVelocity > walkSpeedThreshold
if is_climbing:
_apply_climbing_pose(skel, hips_world_pos, verticalVelocity > 0)
elif is_walking:
_apply_walking_cycle(skel, hips_world_pos, delta)
else:
_apply_idle_stance(skel, hips_world_pos)
func _apply_climbing_pose(skel: Skeleton3D, hips_pos: Vector3, is_up: bool) → void:
# Going up: front leg lifts, back leg extends
# Going down: front leg extends downward, back leg bends
var step_offset = stepHeight if is_up else -stepHeight * 0.5
# Left leg (front leg lifts)
var left_target = hips_pos + Vector3(-0.1, step_offset, 0.2)
_apply_leg_ik(skel, true, left_target)
# Right leg (supporting leg)
var right_target = hips_pos + Vector3(0.1, 0, -0.1)
_apply_leg_ik(skel, false, right_target)
func _apply_walking_cycle(skel: Skeleton3D, hips_pos: Vector3, delta: float) → void:
var floor_height = skel.global_position.y
var cycle = fmod(walkCycleTime, 1.0)
# Calculate lift height
var left_phase = sin(cycle * TAU)
var left_lift = max(0, left_phase) * stepHeight
var right_phase = sin((cycle + 0.5) * TAU)
var right_lift = max(0, right_phase) * stepHeight
# Calculate fore-aft swing
var left_forward_factor = cos(cycle * TAU)
var right_forward_factor = cos((cycle + 0.5) * TAU)
# Get movement direction
var move_dir = (xrCameraNode.global_position - lastHmdPosition).normalized()
move_dir.y = 0
if move_dir.length() < 0.01:
move_dir = -xrCameraNode.global_transform.basis.z # Default forward
move_dir.y = 0
move_dir = move_dir.normalized()
# Lateral vector
var side_dir = move_dir.cross(Vector3.UP).normalized()
# Compute final targets:
# Base position (ground) + lateral offset + fore-aft swing + lift
var left_base = Vector3(hips_pos.x, floor_height, hips_pos.z) - side_dir * 0.15
var left_target = left_base + (move_dir * left_forward_factor * stepLength * 0.5)
left_target.y += left_lift # Only add height here
var right_base = Vector3(hips_pos.x, floor_height, hips_pos.z) + side_dir * 0.15
var right_target = right_base + (move_dir * right_forward_factor * stepLength * 0.5)
right_target.y += right_lift
_apply_leg_ik(skel, true, left_target)
_apply_leg_ik(skel, false, right_target)
func _apply_idle_stance(skel: Skeleton3D, hips_pos: Vector3) → void:
# Get floor height (assume Skeleton root node is on the ground)
var floor_height = skel.global_position.y
# Compute left/right foot positions based on Hips direction
var hips_basis = skel.global_transform.basis
var right_dir = hips_basis.x
# Set foot targets: based on Hips horizontal position, but height fixed at ground
var left_target = Vector3(hips_pos.x, floor_height, hips_pos.z) - right_dir * 0.15
var right_target = Vector3(hips_pos.x, floor_height, hips_pos.z) + right_dir * 0.15
_apply_leg_ik(skel, true, left_target)
_apply_leg_ik(skel, false, right_target)
func apply_leg_ik(skel: Skeleton3D, is_left: bool, target_world_pos: Vector3) → void:
var prefix = "left" if is_left else “right_”
var upleg_idx = boneIdx[prefix + "upleg"]
var leg_idx = boneIdx[prefix + "leg"]
var foot_idx = boneIdx[prefix + "foot"]
if upleg_idx == -1 or leg_idx == -1 or foot_idx == -1:
return
var upper_len = boneLengths.get(prefix + "upleg", 0.4)
var lower_len = boneLengths.get(prefix + "leg", 0.4)
# Knees point forward
var pole_hint = Vector3.FORWARD
_solve_two_bone_ik(skel, upleg_idx, leg_idx, foot_idx, target_world_pos, upper_len, lower_len, pole_hint)
##==============================================================================
##Finger animation
##==============================================================================
func _process_fingers(skel: Skeleton3D, is_left: bool) → void:
var side_key = “Left” if is_left else “Right”
var trigger = leftTrigger if is_left else rightTrigger
var grip = leftGrip if is_left else rightGrip
# Index finger controlled by Trigger
_apply_finger_curl(skel, finger_bones[side_key]["Index"], trigger)
# Middle, ring, pinky controlled by Grip
_apply_finger_curl(skel, finger_bones[side_key]["Middle"], grip)
_apply_finger_curl(skel, finger_bones[side_key]["Ring"], grip)
_apply_finger_curl(skel, finger_bones[side_key]["Pinky"], grip)
# Thumb mixed control
_apply_finger_curl(skel, finger_bones[side_key]["Thumb"], (grip + trigger) * 0.5)
func _apply_finger_curl(skel: Skeleton3D, bone_ids: Array, curl_amount: float) → void:
if bone_ids.is_empty():
return
var curl = curl_amount * fingerCurlStrength
# Finger bending: first segment 30%, second 50%, third 100%
var curl_factors = [0.3, 0.5, 1.0, 1.0]
for i in range(bone_ids.size()):
var bid = bone_ids[i]
var rest_rot = skel.get_bone_rest(bid).basis.get_rotation_quaternion()
# Compute bend rotation
var factor = curl_factors[i] if i < curl_factors.size() else 1.0
var bend_angle = deg_to_rad(90.0) * curl * factor
var bend_rot = Quaternion(fingerBendAxis, bend_angle)
# Slerp from rest pose to bent pose
var final_rot = rest_rot.slerp(rest_rot * bend_rot, curl)
skel.set_bone_pose_rotation(bid, final_rot)