Skeleton Constraints (#1222)

Co-authored-by: Butterscotch! <[email protected]>

gui/public/i18n/en/translation.ftl

@@ -450,6 +450,11 @@ settings-general-fk_settings-leg_tweak-foot_plant-description = Foot-plant rotat
 settings-general-fk_settings-leg_fk = Leg tracking
 settings-general-fk_settings-leg_fk-reset_mounting_feet-description = Enable feet Mounting Reset by tiptoeing.
 settings-general-fk_settings-leg_fk-reset_mounting_feet = Feet Mounting Reset
+settings-general-fk_settings-enforce_joint_constraints = Skeletal Limits
+settings-general-fk_settings-enforce_joint_constraints-enforce_constraints = Enforce constraints
+settings-general-fk_settings-enforce_joint_constraints-enforce_constraints-description = Prevents joints from rotating past their limit
+settings-general-fk_settings-enforce_joint_constraints-correct_constraints = Correct with constraints
+settings-general-fk_settings-enforce_joint_constraints-correct_constraints-description = Correct joint rotations when they push past their limit
 settings-general-fk_settings-arm_fk = Arm tracking
 settings-general-fk_settings-arm_fk-description = Force arms to be tracked from the headset (HMD) even if positional hand data is available.
 settings-general-fk_settings-arm_fk-force_arms = Force arms from HMD

gui/src/components/settings/pages/GeneralSettings.tsx

@@ -69,6 +69,9 @@ interface SettingsForm {
     toeSnap: boolean;
     footPlant: boolean;
     selfLocalization: boolean;
+    usePosition: boolean;
+    enforceConstraints: boolean;
+    correctConstraints: boolean;
   };
   ratios: {
     imputeWaistFromChestHip: number;
@@ -128,6 +131,9 @@ const defaultValues: SettingsForm = {
     toeSnap: false,
     footPlant: true,
     selfLocalization: false,
+    usePosition: true,
+    enforceConstraints: true,
+    correctConstraints: true,
   },
   ratios: {
     imputeWaistFromChestHip: 0.3,
@@ -235,6 +241,9 @@ export function GeneralSettings() {
       toggles.toeSnap = values.toggles.toeSnap;
       toggles.footPlant = values.toggles.footPlant;
       toggles.selfLocalization = values.toggles.selfLocalization;
+      toggles.usePosition = values.toggles.usePosition;
+      toggles.enforceConstraints = values.toggles.enforceConstraints;
+      toggles.correctConstraints = values.toggles.correctConstraints;
       modelSettings.toggles = toggles;
     }
 
@@ -981,6 +990,7 @@ export function GeneralSettings() {
                 )}
               />
             </div>
+
             <Typography color="secondary">
               {l10n.getString(
                 'settings-general-fk_settings-arm_fk-reset_mode-description'
@@ -1033,6 +1043,48 @@ export function GeneralSettings() {
               ></Radio>
             </div>
 
+            <div className="flex flex-col pt-2 pb-3">
+              <Typography bold>
+                {l10n.getString(
+                  'settings-general-fk_settings-enforce_joint_constraints'
+                )}
+              </Typography>
+              <Typography color="secondary">
+                {l10n.getString(
+                  'settings-general-fk_settings-enforce_joint_constraints-enforce_constraints-description'
+                )}
+              </Typography>
+            </div>
+            <div className="grid sm:grid-cols-1 pb-3">
+              <CheckBox
+                variant="toggle"
+                outlined
+                control={control}
+                name="toggles.enforceConstraints"
+                label={l10n.getString(
+                  'settings-general-fk_settings-enforce_joint_constraints-enforce_constraints'
+                )}
+              />
+            </div>
+            <div className="flex flex-col pt-2 pb-3">
+              <Typography color="secondary">
+                {l10n.getString(
+                  'settings-general-fk_settings-enforce_joint_constraints-correct_constraints-description'
+                )}
+              </Typography>
+            </div>
+            <div className="grid sm:grid-cols-1 pb-3">
+              <CheckBox
+                variant="toggle"
+                outlined
+                control={control}
+                name="toggles.correctConstraints"
+                label={l10n.getString(
+                  'settings-general-fk_settings-enforce_joint_constraints-correct_constraints'
+                )}
+              />
+            </div>
+
             {config?.debug && (
               <>
                 <div className="flex flex-col pt-2 pb-3">

server/core/src/main/java/dev/slimevr/autobone/AutoBoneStep.kt

@@ -36,7 +36,7 @@ class AutoBoneStep(
 		// Load server configs into the skeleton
 		skeleton1.loadFromConfig(serverConfig)
 		skeleton2.loadFromConfig(serverConfig)
-		// Disable leg tweaks, this will mess with the resulting positions
+		// Disable leg tweaks and IK solver, these will mess with the resulting positions
 		skeleton1.setLegTweaksEnabled(false)
 		skeleton2.setLegTweaksEnabled(false)
 	}

server/core/src/main/java/dev/slimevr/filtering/QuaternionMovingAverage.kt

@@ -21,6 +21,8 @@ class QuaternionMovingAverage(
 	var amount: Float = 0f,
 	initialRotation: Quaternion = IDENTITY,
 ) {
+	var filteredQuaternion = IDENTITY
+	var filteringImpact = 0f
 	private var smoothFactor = 0f
 	private var predictFactor = 0f
 	private var rotBuffer: CircularArrayList<Quaternion>? = null
@@ -29,7 +31,6 @@ class QuaternionMovingAverage(
 	private val fpsTimer = if (VRServer.instanceInitialized) VRServer.instance.fpsTimer else NanoTimer()
 	private var frameCounter = 0
 	private var lastAmt = 0f
-	var filteredQuaternion = IDENTITY
 
 	init {
 		// amount should range from 0 to 1.
@@ -93,6 +94,8 @@ class QuaternionMovingAverage(
 			// No filtering; just keep track of rotations (for going over 180 degrees)
 			filteredQuaternion = latestQuaternion.twinNearest(smoothingQuaternion)
 		}
+
+		filteringImpact = latestQuaternion.angleToR(filteredQuaternion)
 	}
 
 	@Synchronized

server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java

@@ -190,8 +190,8 @@ public static int createModelSettings(
 				humanPoseManager.getToggle(SkeletonConfigToggles.FOOT_PLANT),
 				humanPoseManager.getToggle(SkeletonConfigToggles.SELF_LOCALIZATION),
 				false,
-				true,
-				true
+				humanPoseManager.getToggle(SkeletonConfigToggles.ENFORCE_CONSTRAINTS),
+				humanPoseManager.getToggle(SkeletonConfigToggles.CORRECT_CONSTRAINTS)
 			);
 		int ratiosOffset = ModelRatios
 			.createModelRatios(

server/core/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.kt

@@ -258,6 +258,8 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
 				hpm.setToggle(SkeletonConfigToggles.TOE_SNAP, toggles.toeSnap())
 				hpm.setToggle(SkeletonConfigToggles.FOOT_PLANT, toggles.footPlant())
 				hpm.setToggle(SkeletonConfigToggles.SELF_LOCALIZATION, toggles.selfLocalization())
+				hpm.setToggle(SkeletonConfigToggles.ENFORCE_CONSTRAINTS, toggles.enforceConstraints())
+				hpm.setToggle(SkeletonConfigToggles.CORRECT_CONSTRAINTS, toggles.correctConstraints())
 			}
 
 			if (ratios != null) {

server/core/src/main/java/dev/slimevr/tracking/processor/Bone.kt

@@ -1,19 +1,23 @@
 package dev.slimevr.tracking.processor
 
+import dev.slimevr.tracking.processor.Constraint.Companion.ConstraintType
+import dev.slimevr.tracking.trackers.Tracker
 import io.github.axisangles.ktmath.Quaternion
 import io.github.axisangles.ktmath.Vector3
+import solarxr_protocol.datatypes.BodyPart
 import java.util.concurrent.CopyOnWriteArrayList
 
 /**
  * Represents a bone composed of 2 joints: headNode and tailNode.
  */
-class Bone(val boneType: BoneType) {
+class Bone(val boneType: BoneType, val rotationConstraint: Constraint) {
 	private val headNode = TransformNode(true)
 	private val tailNode = TransformNode(false)
 	var parent: Bone? = null
 		private set
 	val children: MutableList<Bone> = CopyOnWriteArrayList()
 	var rotationOffset = Quaternion.IDENTITY
+	var attachedTracker: Tracker? = null
 
 	init {
 		headNode.attachChild(tailNode)
@@ -58,6 +62,49 @@ class Bone(val boneType: BoneType) {
 		headNode.update()
 	}
 
+	/**
+	 * Computes the rotations and positions of
+	 * this bone and all of its children while
+	 * enforcing rotation constraints.
+	 */
+	fun updateWithConstraints() {
+		val initialRot = getGlobalRotation()
+		val newRot = rotationConstraint.applyConstraint(initialRot, this)
+		setRotationRaw(newRot)
+		updateThisNode()
+
+		// Correct tracker if applicable. Do not adjust correction for hinge constraints
+		// or the upper chest tracker.
+		if (rotationConstraint.constraintType != ConstraintType.HINGE &&
+			rotationConstraint.constraintType != ConstraintType.LOOSE_HINGE &&
+			boneType.bodyPart != BodyPart.UPPER_CHEST
+		) {
+			val deltaRot = newRot * initialRot.inv()
+			val angle = deltaRot.angleR()
+
+			if (angle > Constraint.ANGLE_THRESHOLD &&
+				(attachedTracker?.filteringHandler?.getFilteringImpact() ?: 1f) < Constraint.FILTER_IMPACT_THRESHOLD &&
+				(parent?.attachedTracker?.filteringHandler?.getFilteringImpact() ?: 0f) < Constraint.FILTER_IMPACT_THRESHOLD
+			) {
+				attachedTracker?.resetsHandler?.updateConstraintFix(deltaRot)
+			}
+		}
+
+		// Recursively apply constraints and update children.
+		for (child in children) {
+			child.updateWithConstraints()
+		}
+	}
+
+	/**
+	 * Computes the rotations and positions of this bone.
+	 * Only to be used while traversing bones from top to bottom.
+	 */
+	private fun updateThisNode() {
+		headNode.updateThisNode()
+		tailNode.updateThisNode()
+	}
+
 	/**
 	 * Returns the world-aligned rotation of the bone
 	 */
@@ -75,6 +122,13 @@ class Bone(val boneType: BoneType) {
 		headNode.localTransform.rotation = rotation * rotationOffset
 	}
 
+	/**
+	 * Sets the global rotation of the bone directly
+	 */
+	fun setRotationRaw(rotation: Quaternion) {
+		headNode.localTransform.rotation = rotation
+	}
+
 	/**
 	 * Returns the global position of the head of the bone
 	 */

server/core/src/main/java/dev/slimevr/tracking/processor/BoneType.java

@@ -36,6 +36,8 @@ public enum BoneType {
 	RIGHT_UPPER_ARM(BodyPart.RIGHT_UPPER_ARM),
 	LEFT_SHOULDER(BodyPart.LEFT_SHOULDER),
 	RIGHT_SHOULDER(BodyPart.RIGHT_SHOULDER),
+	LEFT_UPPER_SHOULDER,
+	RIGHT_UPPER_SHOULDER,
 	LEFT_HAND(BodyPart.LEFT_HAND),
 	RIGHT_HAND(BodyPart.RIGHT_HAND),
 	LEFT_HAND_TRACKER,