Initial step mounting

This commit is contained in:
Butterscotch! committed 2026-09-28 13:48:05 -04:00
1 parent 420aa2c461
commit 0181119829
12 files changed
+561

No files matched your search

@@ -12,6 +12,7 @@ import dev.slimevr.routing.BoneRoutingManager
import dev.slimevr.serial.SerialServer
import dev.slimevr.skeleton.Skeleton
import dev.slimevr.solarxr.rpc.ServerInfos
import dev.slimevr.stepmounting.StepMountingManager
import dev.slimevr.tapdetection.TapDetectionManager
import dev.slimevr.trackingchecklist.TrackingChecklist
import dev.slimevr.udp.UdpServer
@@ -49,6 +50,7 @@ interface AppContextProvider : Phase1ContextProvider {
val vrcOscManager: VRCOSCManager
val resetsManager: ResetsManager
val tapDetectionManager: TapDetectionManager
val stepMountingManager: StepMountingManager
fun startObserving()
suspend fun dispose()
}
@@ -74,6 +76,7 @@ class AppContext(
override val vrcOscManager: VRCOSCManager,
override val resetsManager: ResetsManager,
override val tapDetectionManager: TapDetectionManager,
override val stepMountingManager: StepMountingManager,
) : AppContextProvider {
override fun startObserving() {
keybindManager.startObserving(this)
@@ -90,6 +93,7 @@ class AppContext(
vrcOscManager.startObserving(this)
resetsManager.startObserving()
tapDetectionManager.startObserving()
stepMountingManager.startObserving()
}
override suspend fun dispose() {
@@ -32,6 +32,7 @@ object AppLogger {
val logging = logger("Logging")
val events = logger("Events")
val stayAligned = logger("Stay Aligned")
val stepMounting = logger("Step Mounting")
val coroutines = noCoLogger("Coroutines")
val console = noCoLogger("Console")
@@ -0,0 +1,18 @@
package dev.slimevr.stepmounting
import io.github.axisangles.ktmath.Vector3
class AccelerationIntegrator {
var acceleration = Vector3.ZERO
private set
var velocity = Vector3.ZERO
private set
var offset = Vector3.ZERO
private set
fun integrate(acceleration: Vector3, time: Float) {
this.acceleration = acceleration
offset += (velocity * time) + ((acceleration * time * time) / 2f)
velocity += acceleration * time
}
}
@@ -0,0 +1,164 @@
package dev.slimevr.stepmounting
import dev.slimevr.logging.AppLogger
import dev.slimevr.tracker.Tracker
import dev.slimevr.tracker.TrackerActions
import dev.slimevr.tracker.TrackerState
import dev.slimevr.util.ButterworthCoefficients
import dev.slimevr.util.Vector3Butterworth
import kotlinx.coroutines.ExperimentalCoroutinesApi
import kotlinx.coroutines.delay
import kotlinx.coroutines.flow.Flow
import kotlinx.coroutines.flow.combine
import kotlinx.coroutines.flow.distinctUntilChanged
import kotlinx.coroutines.flow.first
import kotlinx.coroutines.flow.flatMapLatest
import kotlinx.coroutines.flow.flow
import kotlinx.coroutines.flow.flowOf
import kotlinx.coroutines.flow.launchIn
import kotlinx.coroutines.flow.onEach
import kotlinx.coroutines.flow.stateIn
import kotlinx.coroutines.flow.takeWhile
import kotlinx.coroutines.withTimeoutOrNull
import solarxr_protocol.datatypes.BodyPart
import kotlin.time.TimeSource
internal const val TIMEOUT_MS = 30_000L
class StepMountingBasicBehaviour : StepMountingBehaviour {
fun canCalibrate(trackers: List<TrackerState>): Boolean {
val hasPositionalHead = trackers.any { it.bodyPart == BodyPart.HEAD && it.position != null }
return hasPositionalHead
}
@OptIn(ExperimentalCoroutinesApi::class)
override fun observe(receiver: StepMountingManager) {
receiver.server.context.state
.flatMapLatest { state ->
val trackers = state.trackers.values.toList()
if (trackers.isEmpty()) return@flatMapLatest flowOf(false)
// React to per-tracker position/bodyPart changes, not just tracker add/remove. canCalibrate
// only looks at bodyPart and whether position is set, so dedup on that per tracker rather
// than letting every rotation packet resume the combine.
combine(
trackers.map { tracker ->
tracker.context.state.distinctUntilChanged { a, b ->
a.bodyPart == b.bodyPart && (a.position == null) == (b.position == null)
}
},
) { states -> canCalibrate(states.toList()) }
}
.distinctUntilChanged()
.onEach { receiver.context.dispatch(StepMountingActions.SetCanCalibrate(it)) }
.launchIn(receiver.context.scope)
}
}
val coefficients = ButterworthCoefficients(
8f,
0.02f,
)
const val startThreshold = 0.4f // in m/s^2
const val endThreshold = 0.2f // in m/s^2
const val minMovementDurationMs = 3000L
fun movementDetector(updates: Flow<TrackerSnapshot>) = flow {
val lowpass = Vector3Butterworth(coefficients)
emit(0f)
updates.collect { update ->
val lowpassAccel = lowpass.filter(update.acceleration)
emit((update.acceleration - lowpassAccel).len())
}
}
internal suspend fun runCalibrationSession(
context: StepMountingContext,
headUpdates: Flow<HeadsetSnapshot>,
trackers: List<Pair<Tracker, Flow<TrackerSnapshot>>>,
clock: TimeSource.WithComparableMarks = TimeSource.Monotonic,
) {
val headRecording = ArrayList<HeadRecordingSample>(1024)
val trackerRecordings = ArrayList<Pair<Tracker, ArrayList<RecordingSample>>>(trackers.size)
fun dispatch(status: StepMountingStatus) {
context.dispatch(StepMountingActions.Update(status))
}
dispatch(StepMountingStatus.WAITING_FOR_MOVEMENT)
AppLogger.stepMounting.info("Waiting for movement...")
withTimeoutOrNull(TIMEOUT_MS) {
val isMoving = combine(
trackers.map { (_, updates) ->
movementDetector(updates)
},
) { updates ->
// Max tracker movement
updates.max()
}.stateIn(this)
// Wait for movement to start
isMoving.first { it >= startThreshold }
dispatch(StepMountingStatus.RECORDING)
AppLogger.stepMounting.info("Movement detected, recording...")
// Collect head and tracker data
headUpdates.takeWhile {
context.state.value.status == StepMountingStatus.RECORDING
}.onEach { update ->
headRecording.add(
HeadRecordingSample(
time = clock.markNow(),
position = update.position,
),
)
}.launchIn(this)
for ((tracker, updates) in trackers) {
val recording = ArrayList<RecordingSample>(1024)
trackerRecordings.add(tracker to recording)
updates.takeWhile {
context.state.value.status == StepMountingStatus.RECORDING
}.onEach { update ->
recording.add(
RecordingSample(
time = clock.markNow(),
accel = update.acceleration,
),
)
}.launchIn(this)
}
// Wait for movement to end
delay(minMovementDurationMs)
isMoving.first { it < endThreshold }
dispatch(StepMountingStatus.PROCESSING)
AppLogger.stepMounting.info("No more movement detected, processing...")
if (headRecording.isEmpty() || trackerRecordings.isEmpty()) {
dispatch(StepMountingStatus.ERROR_NO_DATA)
error("No data found.")
}
val headOffset = headRecording.last().position - headRecording.first().position
trackerRecordings.filter {
it.second.isNotEmpty()
}.map { (tracker, recording) ->
tracker to estimateHeadingAlign(recording, headOffset)
}.forEach { (tracker, result) ->
// TODO: Fail on high error
AppLogger.stepMounting.info(result)
tracker.context.dispatch(
TrackerActions.SetStepMounting(
result.headingAlignment,
),
)
}
dispatch(StepMountingStatus.DONE)
AppLogger.stepMounting.info("Done!")
} ?: dispatch(StepMountingStatus.ERROR_TIMEOUT)
}
@@ -0,0 +1,146 @@
@file:OptIn(kotlinx.coroutines.ExperimentalCoroutinesApi::class)
package dev.slimevr.stepmounting
import dev.slimevr.Phase1ContextProvider
import dev.slimevr.VRServer
import dev.slimevr.config.Settings
import dev.slimevr.context.Behaviour
import dev.slimevr.context.Context
import dev.slimevr.tracker.Tracker
import io.github.axisangles.ktmath.Quaternion
import io.github.axisangles.ktmath.Vector3
import kotlinx.coroutines.CoroutineScope
import kotlinx.coroutines.Job
import kotlinx.coroutines.flow.Flow
import kotlinx.coroutines.flow.emptyFlow
import kotlinx.coroutines.flow.flatMapConcat
import kotlinx.coroutines.flow.map
import kotlinx.coroutines.launch
import solarxr_protocol.datatypes.BodyPart
import solarxr_protocol.datatypes.TrackerStatus
import kotlin.collections.emptyList
import kotlin.collections.listOf
import kotlin.collections.map
import kotlin.time.ComparableTimeMark
// TODO Put this enum in SolarXR
enum class StepMountingStatus(
val `value`: UByte,
) {
NONE(0.toUByte()),
WAITING_FOR_MOVEMENT(1.toUByte()),
RECORDING(2.toUByte()),
PROCESSING(3.toUByte()),
DONE(4.toUByte()),
ERROR_NO_DATA(5.toUByte()),
ERROR_TIMEOUT(6.toUByte()),
;
companion object {
fun fromValue(`value`: UByte): StepMountingStatus? = entries.firstOrNull { it.value == value }
}
}
data class HeadsetSnapshot(
val rotation: Quaternion,
val position: Vector3,
)
data class TrackerSnapshot(
val rotation: Quaternion,
val acceleration: Vector3,
)
data class HeadRecordingSample(
val time: ComparableTimeMark,
val position: Vector3,
)
data class RecordingSample(
val time: ComparableTimeMark,
val accel: Vector3,
)
data class StepMountingState(
val status: StepMountingStatus,
val canDoStepMounting: Boolean,
)
sealed interface StepMountingActions {
data class Update(val status: StepMountingStatus) : StepMountingActions
data class SetCanCalibrate(val canDo: Boolean) : StepMountingActions
}
typealias StepMountingContext = Context<StepMountingState, StepMountingActions>
typealias StepMountingBehaviour = Behaviour<StepMountingManager>
class StepMountingManager(
val context: StepMountingContext,
val server: VRServer,
val settings: Settings,
) {
fun startObserving() = context.observeAll(this)
private var sessionJob: Job? = null
// These Flows do nothing until the calibration uses collect on it
val headUpdates: Flow<HeadsetSnapshot> = server.context.state
.flatMapConcat { state ->
val positionalHead = state.trackers.values
.find {
val state = it.context.state.value
state.bodyPart == BodyPart.HEAD && state.status == TrackerStatus.OK && state.position != null
}
?: return@flatMapConcat emptyFlow()
positionalHead.context.state.map { s ->
HeadsetSnapshot(
rotation = s.rotation,
position = s.position ?: error("Head will always have a position here"),
)
}
}
fun getTrackers(): List<Pair<Tracker, Flow<TrackerSnapshot>>> {
val activeTrackers = server.context.state.value.trackers.values.filter {
val state = it.context.state.value
state.status == TrackerStatus.OK && state.acceleration != null
}
if (activeTrackers.isEmpty()) return emptyList()
return activeTrackers.map { controller ->
controller to controller.context.state.map { s ->
TrackerSnapshot(
rotation = s.rotation,
acceleration = s.acceleration ?: error("Trackers will always have acceleration in this case"),
)
}
}
}
fun start() {
sessionJob?.cancel()
sessionJob = context.scope.launch { runCalibrationSession(context, headUpdates, getTrackers()) }
}
fun cancel() {
sessionJob?.cancel()
sessionJob = null
context.dispatch(StepMountingActions.Update(StepMountingStatus.NONE))
}
companion object {
fun create(ctx: Phase1ContextProvider, scope: CoroutineScope): StepMountingManager {
val context = Context.create(
initialState = StepMountingState(
status = StepMountingStatus.NONE,
canDoStepMounting = false,
),
scope = scope,
reducer = ::reduce,
behaviours = listOf(StepMountingBasicBehaviour()),
name = "StepMountingManager",
)
return StepMountingManager(context, ctx.server, ctx.config.settings)
}
}
}
@@ -0,0 +1,81 @@
package dev.slimevr.stepmounting
import dev.slimevr.tracker.HeadingAlignment
import dev.slimevr.util.inFloatingSeconds
import io.github.axisangles.ktmath.Quaternion
import io.github.axisangles.ktmath.Vector3
import kotlin.math.atan2
import kotlin.time.ComparableTimeMark
import kotlin.time.Duration
fun accumSample(
accum: AccelerationIntegrator,
sample: RecordingSample,
lastSampleTime: ComparableTimeMark? = null,
accelBias: Vector3 = Vector3.ZERO,
): Duration {
val delta = lastSampleTime?.let { sample.time - it } ?: Duration.ZERO
accum.integrate(sample.accel - accelBias, delta.inFloatingSeconds)
return delta
}
fun processTimeline(
accum: AccelerationIntegrator,
recording: Iterable<RecordingSample>,
lastSampleTime: ComparableTimeMark? = null,
accelBias: Vector3 = Vector3.ZERO,
): ComparableTimeMark? {
var lastTime = lastSampleTime
for (sample in recording) {
accumSample(accum, sample, lastTime, accelBias)
lastTime = sample.time
}
return lastTime
}
fun angle(vector: Vector3): Quaternion {
val yaw = atan2(vector.x, vector.z)
return Quaternion.rotationAroundYAxis(yaw)
}
data class StepMountingResult(
val headingAlignment: HeadingAlignment,
val errorMeters: Float,
val trackerOffset: Vector3,
val accelerationBias: Vector3,
)
fun estimateHeadingAlign(
recording: Iterable<RecordingSample>,
headOffset: Vector3,
): StepMountingResult {
// Compute the unbiased final velocity
val calibAccum = AccelerationIntegrator()
processTimeline(calibAccum, recording)
// Assume the final velocity is zero (at rest), we can divide our unbiased final
// velocity (m/s) by the duration and get a static acceleration offset (m/s^2)
val duration = recording.last().time - recording.first().time
val bias = calibAccum.velocity / duration.inFloatingSeconds
// Compute the biased final offset
val finalAccum = AccelerationIntegrator()
processTimeline(finalAccum, recording, accelBias = bias)
// Compute the final offsets
val trackerOffset = finalAccum.offset
val trackerXZ = Vector3(trackerOffset.x, 0f, trackerOffset.z)
val hmdXZ = Vector3(headOffset.x, 0f, headOffset.z)
// Compute mounting to fix the yaw offset from tracker to HMD
// OLD: angle(trackerXZ.unit()) * angle(hmdXZ.unit()).inv()
return StepMountingResult(
Quaternion.fromTo(trackerXZ.unit(), hmdXZ.unit()),
trackerXZ.len() - hmdXZ.len(),
trackerOffset,
bias,
)
}
@@ -0,0 +1,11 @@
package dev.slimevr.stepmounting
fun reduce(state: StepMountingState, action: StepMountingActions): StepMountingState = when (action) {
is StepMountingActions.Update -> state.copy(
status = action.status,
)
is StepMountingActions.SetCanCalibrate -> state.copy(
canDoStepMounting = action.canDo,
)
}
@@ -120,6 +120,7 @@ sealed interface TrackerActions {
data class SetMotion(val motion: Motion) : TrackerActions
data class SetYawCorrection(val yawCorrection: Angle) : TrackerActions
data class SetStayAlignedEnabled(val enabled: Boolean) : TrackerActions
data class SetStepMounting(val headingAlignment: HeadingAlignment) : TrackerActions
}
typealias TrackerContext = Context<TrackerState, TrackerActions>
@@ -244,4 +244,14 @@ fun reduce(
)
}
}
is TrackerActions.SetStepMounting -> {
val newHeadingAlignment = state.sessionCalibration.headingAlignment * action.headingAlignment
state.copy(
sessionCalibration = state.sessionCalibration.copy(headingAlignment = newHeadingAlignment),
lastMountingMethod = MountingMethod.POSE, // TODO More methods?
rotationDirty = true,
pendingSkeletonResets = state.pendingSkeletonResets + ResetType.POSE_MOUNTING,
)
}
}
@@ -0,0 +1,120 @@
package dev.slimevr.util
import com.jme3.math.FastMath
import io.github.axisangles.ktmath.Vector3
import kotlin.math.sqrt
import kotlin.math.tan
data class ButterworthCoefficients(
/** Corresponds to cutoff frequency (Hz) as `1f / tau`. **/
val tau: Float,
/** Sampling period in seconds. **/
val Ts: Float,
) {
val b0: Float
val b1: Float
val b2: Float
val a1: Float
val a2: Float
init {
require(tau > 0f) { "tau must be > 0" }
require(Ts > 0f) { "Ts must be > 0" }
val fc = (sqrt(2f) / FastMath.TWO_PI) / tau
val C = tan(FastMath.PI * fc * Ts)
val D = (C * C) + (sqrt(2f) * C) + 1f
val b0 = (C * C) / D
this.b0 = b0
this.b1 = 2f * b0
this.b2 = b0
// a0 = 1f
this.a1 = (2f * ((C * C) - 1f)) / D
this.a2 = ((1f - (sqrt(2f) * C)) + (C * C)) / D
}
}
class Butterworth(
coefficients: ButterworthCoefficients,
) {
var coefficients = coefficients
private set
private var initialized = false
private var y: Float = 0f
private var state0: Float = 0f
private var state1: Float = 0f
fun swapCoefficients(coefficients: ButterworthCoefficients) {
// Don't do anything if we're not initialized
if (!initialized) {
this.coefficients = coefficients
return
}
val old = this.coefficients
val new = coefficients
state0 += (old.b0 - new.b0) * y
state1 += (old.b1 - new.b1 - old.a1 + new.a1) * y
this.coefficients = coefficients
}
fun filter(
x: Float,
): Float {
// Average through duration tau to initialize
if (!initialized) {
state0 += 1f // Sample count
state1 += x // Sum
val out = state1 / state0
// If reached tau, initialize
if (state0 * coefficients.Ts >= coefficients.tau) {
state0 = out * (1f - coefficients.b0)
state1 = out * (coefficients.b2 - coefficients.a2)
initialized = true
}
return out
}
y = coefficients.b0 * x + state0
state0 = coefficients.b1 * x - coefficients.a1 * y + state1
state1 = coefficients.b2 * x - coefficients.a2 * y
return y
}
}
class Vector3Butterworth(
coefficients: ButterworthCoefficients,
) {
private val x = Butterworth(coefficients)
private val y = Butterworth(coefficients)
private val z = Butterworth(coefficients)
val coefficients
get() = x.coefficients
fun swapCoefficients(
coefficients: ButterworthCoefficients,
) {
x.swapCoefficients(coefficients)
y.swapCoefficients(coefficients)
z.swapCoefficients(coefficients)
}
fun filter(
x: Vector3,
): Vector3 = Vector3(
this.x.filter(x.x),
this.y.filter(x.y),
this.z.filter(x.z),
)
}
@@ -30,6 +30,7 @@ import dev.slimevr.skeleton.ProportionsBehaviour
import dev.slimevr.skeleton.Skeleton
import dev.slimevr.skeleton.buildBones
import dev.slimevr.solarxr.rpc.ServerInfos
import dev.slimevr.stepmounting.StepMountingManager
import dev.slimevr.tapdetection.TapDetectionManager
import dev.slimevr.tracker.Motion
import dev.slimevr.tracker.SessionCalibration
@@ -256,6 +257,7 @@ abstract class TestAppContext : AppContextProvider {
override val resetsManager: ResetsManager get() = error("not used in test")
override val tapDetectionManager: TapDetectionManager get() = error("not used in test")
override val boneRouting: BoneRoutingManager get() = error("not used in test")
override val stepMountingManager: StepMountingManager get() = error("not used in test")
override fun startObserving() {}
override suspend fun dispose() = Unit
}
@@ -40,6 +40,7 @@ import dev.slimevr.resolveConfigDirectory
import dev.slimevr.routing.BoneRoutingManager
import dev.slimevr.skeleton.Skeleton
import dev.slimevr.solarxr.rpc.ServerInfos
import dev.slimevr.stepmounting.StepMountingManager
import dev.slimevr.tapdetection.TapDetectionManager
import dev.slimevr.trackingchecklist.TrackingChecklist
import dev.slimevr.udp.UdpServer
@@ -155,6 +156,7 @@ fun main(args: Array<String>) = runBlocking<Unit>(appCoroutineExceptionHandler +
)
val resetsManager = ResetsManager.create(ctx = phase1, skeleton = skeleton, scope = this)
val tapDetectionManager = TapDetectionManager.create(ctx = phase1, resetsManager = resetsManager, scope = this)
val stepMountingManager = StepMountingManager.create(ctx = phase1, scope = this)
val keybindManager = KeybindManager.create(scope = this)
val serverInfos = ServerInfos(::resolveDesktopLocalIpAddress)
@@ -179,6 +181,7 @@ fun main(args: Array<String>) = runBlocking<Unit>(appCoroutineExceptionHandler +
vrcOscManager = vrcOscManager,
resetsManager = resetsManager,
tapDetectionManager = tapDetectionManager,
stepMountingManager = stepMountingManager,
)
try {