[Spine Yaw Compensation] Add small yaw rotations to keep spine trackers aligned

Author: jabberrock

gui/public/i18n/en/translation.ftl

@@ -373,6 +373,13 @@ settings-general-tracker_mechanics-use_mag_on_all_trackers-description =
     Uses magnetometer on all trackers that have a compatible firmware for it, reducing drift in stable magnetic environments.
     Can be disabled per tracker in the tracker's settings. <b>Please don't shutdown any of the trackers while toggling this!</b>
 settings-general-tracker_mechanics-use_mag_on_all_trackers-label = Use magnetometer on trackers
+settings-general-tracker_mechanics-spine_yaw_correction = Keep Spine Aligned
+settings-general-tracker_mechanics-spine_yaw_correction-description =
+    Prevents the spine trackers from drifting, by nudging them to stay aligned to the headset.
+    Players are usually standing facing forwards or walking forwards, where the spine trackers are aligned to the yaw of the headset. However due to yaw drift, the spine trackers will eventually rotate to the side, requiring a reset. This feature adds a small centering rotation to compensate for the yaw drift. (For now, this compensation only activates when your trackers are pointing "up", e.g. when you're standing or sitting straight up.)
+settings-general-tracker_mechanics-spine_yaw_correction-enabled-label = Enabled
+settings-general-tracker_mechanics-spine_yaw_correction-amount-label = Amount
+settings-general-tracker_mechanics-spine_yaw_correction-amount-description = The correction amount should be approximately the maximum yaw drift of your gyroscope (0.3 degrees/sec is a good default).
 
 ## FK/Tracking settings
 settings-general-fk_settings = Tracking settings

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

@@ -16,6 +16,7 @@ import {
   SettingsResponseT,
   SteamVRTrackersSettingT,
   TapDetectionSettingsT,
+  YawCorrectionSettingsT,
 } from 'solarxr-protocol';
 import { useConfig } from '@/hooks/config';
 import { useWebsocketAPI } from '@/hooks/websocket-api';
@@ -101,6 +102,10 @@ interface SettingsForm {
     saveMountingReset: boolean;
     resetHmdPitch: boolean;
   };
+  yawCorrectionSettings: {
+    enabled: boolean;
+    amountInDegPerSec: number;
+  };
 }
 
 const defaultValues: SettingsForm = {
@@ -165,6 +170,10 @@ const defaultValues: SettingsForm = {
     saveMountingReset: false,
     resetHmdPitch: false,
   },
+  yawCorrectionSettings: {
+    enabled: false,
+    amountInDegPerSec: 0.0,
+  },
 };
 
 export function GeneralSettings() {
@@ -184,6 +193,10 @@ export function GeneralSettings() {
     unitDisplay: 'narrow',
     maximumFractionDigits: 2,
   });
+  const degreeFormat = new Intl.NumberFormat(currentLocales, {
+    style: 'unit',
+    unit: 'degree',
+  });
 
   const { sendRPCPacket, useRPCPacket } = useWebsocketAPI();
   const { reset, control, watch, handleSubmit, getValues, setValue } =
@@ -291,6 +304,12 @@ export function GeneralSettings() {
     driftCompensation.maxResets = values.driftCompensation.maxResets;
     settings.driftCompensation = driftCompensation;
 
+    const yawCorrectionSettings = new YawCorrectionSettingsT();
+    yawCorrectionSettings.enabled = values.yawCorrectionSettings.enabled;
+    yawCorrectionSettings.amountInDegPerSec =
+      values.yawCorrectionSettings.amountInDegPerSec;
+    settings.yawCorrectionSettings = yawCorrectionSettings;
+
     if (values.resetsSettings) {
       const resetsSettings = new ResetsSettingsT();
       resetsSettings.resetMountingFeet =
@@ -414,6 +433,10 @@ export function GeneralSettings() {
       formData.resetsSettings = settings.resetsSettings;
     }
 
+    if (settings.yawCorrectionSettings) {
+      formData.yawCorrectionSettings = settings.yawCorrectionSettings;
+    }
+
     reset({ ...getValues(), ...formData });
   });
 
@@ -813,6 +836,55 @@ export function GeneralSettings() {
               settingType="general"
               id="mechanics-magnetometer"
             />
+            <div className="flex flex-col pt-4 pb-4"></div>
+            <Typography bold>
+              {l10n.getString(
+                'settings-general-tracker_mechanics-spine_yaw_correction'
+              )}
+            </Typography>
+            <div className="flex flex-col pt-2 pb-4">
+              {l10n
+                .getString(
+                  'settings-general-tracker_mechanics-spine_yaw_correction-description'
+                )
+                .split('\n')
+                .map((line, i) => (
+                  <Typography color="secondary" key={i}>
+                    {line}
+                  </Typography>
+                ))}
+            </div>
+            <div className="grid sm:grid-cols-1 gap-3 pb-4">
+              <CheckBox
+                variant="toggle"
+                outlined
+                control={control}
+                name="yawCorrectionSettings.enabled"
+                label={l10n.getString(
+                  'settings-general-tracker_mechanics-spine_yaw_correction-enabled-label'
+                )}
+              />
+              <div>
+                <Typography bold>
+                  {l10n.getString(
+                    'settings-general-tracker_mechanics-spine_yaw_correction-amount-label'
+                  )}
+                </Typography>
+                <Typography color="secondary">
+                  {l10n.getString(
+                    'settings-general-tracker_mechanics-spine_yaw_correction-amount-description'
+                  )}
+                </Typography>
+                <NumberSelector
+                  control={control}
+                  name="yawCorrectionSettings.amountInDegPerSec"
+                  valueLabelFormat={(value) => degreeFormat.format(value)}
+                  min={0.0}
+                  max={2.0}
+                  step={0.05}
+                />
+              </div>
+            </div>
           </>
         </SettingsPagePaneLayout>
         <SettingsPagePaneLayout

server/core/src/main/java/dev/slimevr/config/VRConfig.kt

@@ -40,6 +40,8 @@ class VRConfig {
 
 	val resetsConfig: ResetsConfig = ResetsConfig()
 
+	val yawCorrectionConfig = YawCorrectionConfig()
+
 	@JsonDeserialize(using = TrackerConfigMapDeserializer::class)
 	@JsonSerialize(keyUsing = StdKeySerializers.StringKeySerializer::class)
 	private val trackers: MutableMap<String, TrackerConfig> = HashMap()

server/core/src/main/java/dev/slimevr/config/YawCorrectionConfig.kt

@@ -0,0 +1,8 @@
+package dev.slimevr.config
+
+class YawCorrectionConfig {
+
+	var enabled = false
+
+	var amountInDegPerSec = 0.3f
+}

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

@@ -349,6 +349,18 @@ public static int createArmsResetModeSettings(
 			);
 	}
 
+	public static int createYawCorrectionSettings(
+		FlatBufferBuilder fbb,
+		YawCorrectionConfig yawCorrectionConfig
+	) {
+		return YawCorrectionSettings
+			.createYawCorrectionSettings(
+				fbb,
+				yawCorrectionConfig.getEnabled(),
+				yawCorrectionConfig.getAmountInDegPerSec()
+			);
+	}
+
 	public static int createSettingsResponse(FlatBufferBuilder fbb, VRServer server) {
 		ISteamVRBridge bridge = server.getVRBridge(ISteamVRBridge.class);
 
@@ -401,6 +413,11 @@ public static int createSettingsResponse(FlatBufferBuilder fbb, VRServer server)
 					.createArmsResetModeSettings(
 						fbb,
 						server.configManager.getVrConfig().getResetsConfig()
+					),
+				RPCSettingsBuilder
+					.createYawCorrectionSettings(
+						fbb,
+						server.configManager.getVrConfig().getYawCorrectionConfig()
 					)
 			);
 	}

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

@@ -345,6 +345,15 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
 			resetsConfig.updateTrackersResetsSettings()
 		}
 
+		if (req.yawCorrectionSettings() != null) {
+			val yawCorrectionConfig = api.server.configManager
+				.vrConfig
+				.yawCorrectionConfig
+			yawCorrectionConfig.enabled = req.yawCorrectionSettings().enabled()
+			yawCorrectionConfig.amountInDegPerSec = req.yawCorrectionSettings().amountInDegPerSec()
+			api.server.humanPoseManager.setYawCorrection(yawCorrectionConfig)
+		}
+
 		api.server.configManager.saveConfig()
 	}
 
@@ -361,7 +370,7 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
 			val settings = SettingsResponse
 				.createSettingsResponse(
 					fbb,
-					RPCSettingsBuilder.createSteamVRSettings(fbb, bridge), 0, 0, 0, 0, 0, 0, 0, 0, 0,
+					RPCSettingsBuilder.createSteamVRSettings(fbb, bridge), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
 				)
 			val outbound =
 				rpcHandler.createRPCMessage(fbb, RpcMessage.SettingsResponse, settings)

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

@@ -4,6 +4,7 @@ import com.jme3.math.FastMath
 import dev.slimevr.VRServer
 import dev.slimevr.VRServer.Companion.getNextLocalTrackerId
 import dev.slimevr.config.ConfigManager
+import dev.slimevr.config.YawCorrectionConfig
 import dev.slimevr.tracking.processor.config.SkeletonConfigManager
 import dev.slimevr.tracking.processor.config.SkeletonConfigOffsets
 import dev.slimevr.tracking.processor.config.SkeletonConfigToggles
@@ -286,6 +287,7 @@ class HumanPoseManager(val server: VRServer?) {
 
 	fun loadFromConfig(configManager: ConfigManager) {
 		skeletonConfigManager.loadFromConfig(configManager)
+		skeleton.setYawCorrectionConfig(configManager.vrConfig.yawCorrectionConfig)
 	}
 
 	@VRServerThread
@@ -663,6 +665,10 @@ class HumanPoseManager(val server: VRServer?) {
 		}
 	}
 
+	fun setYawCorrection(yawCorrectionConfig: YawCorrectionConfig) {
+		skeleton.setYawCorrectionConfig(yawCorrectionConfig)
+	}
+
 	fun setLegTweaksStateTemp(
 		skatingCorrection: Boolean,
 		floorClip: Boolean,

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

@@ -2,6 +2,7 @@ package dev.slimevr.tracking.processor.skeleton
 
 import com.jme3.math.FastMath
 import dev.slimevr.VRServer
+import dev.slimevr.config.YawCorrectionConfig
 import dev.slimevr.tracking.processor.Bone
 import dev.slimevr.tracking.processor.BoneType
 import dev.slimevr.tracking.processor.HumanPoseManager
@@ -141,12 +142,14 @@ class HumanSkeleton(
 
 	// Others
 	private var pauseTracking = false // Pauses skeleton tracking if true, resumes skeleton tracking if false
+	var yawCorrectionInDegPerSec = 0.0f
 
 	// Modules
 	var legTweaks = LegTweaks(this)
 	var tapDetectionManager = TapDetectionManager(this)
 	var viveEmulation = ViveEmulation(this)
 	var localizer = Localizer(this)
+	var spineYawCorrection = SpineYawCorrection(this, YawCorrectionConfig())
 
 	// Constructors
 	init {
@@ -360,6 +363,9 @@ class HumanSkeleton(
 	fun updatePose() {
 		tapDetectionManager.update()
 
+		// Need to update tracker rotations before we calculate the new skeleton poses
+		spineYawCorrection.updateTrackers()
+
 		updateTransforms()
 		updateBones()
 		updateComputedTrackers()
@@ -1239,6 +1245,10 @@ class HumanSkeleton(
 		return newState
 	}
 
+	fun setYawCorrectionConfig(config: YawCorrectionConfig) {
+		spineYawCorrection = SpineYawCorrection(this, config)
+	}
+
 	companion object {
 		val FORWARD_QUATERNION = EulerAngles(
 			EulerOrder.YZX,

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

@@ -0,0 +1,76 @@
+package dev.slimevr.tracking.processor.skeleton
+
+import com.jme3.math.FastMath
+import dev.slimevr.VRServer
+import dev.slimevr.config.YawCorrectionConfig
+import dev.slimevr.tracking.trackers.Tracker
+import io.github.axisangles.ktmath.EulerOrder
+import io.github.axisangles.ktmath.Quaternion
+import io.github.axisangles.ktmath.Vector3.Companion.POS_Y
+import kotlin.math.*
+
+class SpineYawCorrection(
+	val skeleton: HumanSkeleton,
+	private val yawCorrectionConfig: YawCorrectionConfig,
+) {
+
+	private val upperBodyTrackers = filterImuTrackers(
+		skeleton.headTracker,
+		skeleton.neckTracker,
+		skeleton.upperChestTracker,
+		skeleton.chestTracker,
+		skeleton.waistTracker,
+		skeleton.hipTracker,
+	)
+
+	fun updateTrackers() {
+		if (!yawCorrectionConfig.enabled) {
+			return
+		}
+
+		updateTrackers(upperBodyTrackers)
+	}
+
+	private fun updateTrackers(trackers: List<Tracker>) {
+		for ((parentTracker, tracker) in trackers.zipWithNext()) {
+			updateTracker(tracker, parentTracker)
+		}
+	}
+
+	private fun updateTracker(tracker: Tracker, parentTracker: Tracker) {
+		val trackerRot = tracker.getRotation()
+		val parentTrackerRot = parentTracker.getRotation()
+
+		// For now, we only handle trackers which are relatively "upright", i.e. in the
+		// standing position, or sitting position for trackers that are high up on the
+		// spine. When someone is lying down, they could be curled up such that the yaws
+		// don't necessarily align.
+		val maxDeviationInRad = 30.0f * FastMath.DEG_TO_RAD
+		if (!isTrackerPointingUp(trackerRot, maxDeviationInRad) ||
+			!isTrackerPointingUp(parentTrackerRot, maxDeviationInRad)
+		) {
+			return
+		}
+
+		val deltaRot = trackerRot * parentTrackerRot.inv()
+		val deltaYawInRad = deltaRot.toEulerAngles(EulerOrder.YZX).y
+
+		// Amount of yaw should be roughly the maximum yaw bias of the gyroscope. If it
+		// is too small, the gyroscope will overpower the correction and the skeleton
+		// will drift. If it is too big, the player will notice that the skeleton is
+		// rotating when the player doesn't face forward for a long time.
+		val adjustYawInRad = -sign(deltaYawInRad) * (yawCorrectionConfig.amountInDegPerSec * FastMath.DEG_TO_RAD) * VRServer.instance.fpsTimer.timePerFrame
+
+		// Adjust the tracker's yaw towards the parent tracker's yaw
+		tracker.resetsHandler.spineYawCorrectionInRad += adjustYawInRad
+	}
+
+	private fun isTrackerPointingUp(trackerRot: Quaternion, maxDeviationInRad: Float): Boolean {
+		val trackerUp = trackerRot.sandwich(POS_Y)
+		return trackerUp.angleTo(POS_Y) < maxDeviationInRad
+	}
+
+	companion object {
+		private fun filterImuTrackers(vararg trackers: Tracker?): List<Tracker> = trackers.filterNotNull().filter { it.isImu() }.toList()
+	}
+}

server/core/src/main/java/dev/slimevr/tracking/trackers/Tracker.kt

@@ -333,6 +333,8 @@ class Tracker @JvmOverloads constructor(
 			rot = resetsHandler.getReferenceAdjustedDriftRotationFrom(rot)
 		}
 
+		rot = resetsHandler.applySpineYawCorrection(rot)
+
 		return rot
 	}
 
@@ -365,6 +367,8 @@ class Tracker @JvmOverloads constructor(
 			rot = resetsHandler.getIdentityAdjustedDriftRotationFrom(rot)
 		}
 
+		rot = resetsHandler.applySpineYawCorrection(rot)
+
 		return rot
 	}