Skip to content

Commit 0a08d57

Browse files
StermereEirenlielloucass003
authored
Positional tracker support (#920)
Co-authored-by: Eiren Rain <Eirenliel@users.noreply.github.com> Co-authored-by: lucas lelievre <loucass003@gmail.com>
1 parent c316c50 commit 0a08d57

File tree

9 files changed

+472
-2
lines changed

9 files changed

+472
-2
lines changed

‎gui/public/i18n/en/translation.ftl‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -593,6 +593,9 @@ settings-general-fk_settings-enforce_joint_constraints-enforce_constraints = Enf
593593
settings-general-fk_settings-enforce_joint_constraints-enforce_constraints-description = Prevents joints from rotating past their limit
594594
settings-general-fk_settings-enforce_joint_constraints-correct_constraints = Correct with constraints
595595
settings-general-fk_settings-enforce_joint_constraints-correct_constraints-description = Correct joint rotations when they push past their limit
596+
settings-general-fk_settings-ik = Position data
597+
settings-general-fk_settings-ik-use_position = Use Position data
598+
settings-general-fk_settings-ik-use_position-description = Enables the use of position data from trackers that provide it. When enabling this make sure to full reset and recalibrate in game.
596599
settings-general-fk_settings-arm_fk = Arm tracking
597600
settings-general-fk_settings-arm_fk-description = Force arms to be tracked from the headset (HMD) even if positional hand data is available.
598601
settings-general-fk_settings-arm_fk-force_arms = Force arms from HMD

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

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -958,6 +958,28 @@ export function GeneralSettings() {
958958
/>
959959
</div>
960960

961+
<div className="flex flex-col pt-2 pb-3">
962+
<Typography bold>
963+
{l10n.getString('settings-general-fk_settings-ik')}
964+
</Typography>
965+
<Typography color="secondary">
966+
{l10n.getString(
967+
'settings-general-fk_settings-ik-use_position-description'
968+
)}
969+
</Typography>
970+
</div>
971+
<div className="grid sm:grid-cols-1 pb-3">
972+
<CheckBox
973+
variant="toggle"
974+
outlined
975+
control={control}
976+
name="toggles.usePosition"
977+
label={l10n.getString(
978+
'settings-general-fk_settings-ik-use_position'
979+
)}
980+
/>
981+
</div>
982+
961983
{config?.debug && (
962984
<>
963985
<div className="flex flex-col pt-2 pb-3">

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

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -189,7 +189,7 @@ public static int createModelSettings(
189189
humanPoseManager.getToggle(SkeletonConfigToggles.TOE_SNAP),
190190
humanPoseManager.getToggle(SkeletonConfigToggles.FOOT_PLANT),
191191
humanPoseManager.getToggle(SkeletonConfigToggles.SELF_LOCALIZATION),
192-
false,
192+
humanPoseManager.getToggle(SkeletonConfigToggles.USE_POSITION),
193193
humanPoseManager.getToggle(SkeletonConfigToggles.ENFORCE_CONSTRAINTS),
194194
humanPoseManager.getToggle(SkeletonConfigToggles.CORRECT_CONSTRAINTS)
195195
);

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

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -253,6 +253,7 @@ class RPCSettingsHandler(var rpcHandler: RPCHandler, var api: ProtocolAPI) {
253253
hpm.setToggle(SkeletonConfigToggles.TOE_SNAP, toggles.toeSnap())
254254
hpm.setToggle(SkeletonConfigToggles.FOOT_PLANT, toggles.footPlant())
255255
hpm.setToggle(SkeletonConfigToggles.SELF_LOCALIZATION, toggles.selfLocalization())
256+
hpm.setToggle(SkeletonConfigToggles.USE_POSITION, toggles.usePosition())
256257
hpm.setToggle(SkeletonConfigToggles.ENFORCE_CONSTRAINTS, toggles.enforceConstraints())
257258
hpm.setToggle(SkeletonConfigToggles.CORRECT_CONSTRAINTS, toggles.correctConstraints())
258259
}

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

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -588,6 +588,11 @@ class HumanPoseManager(val server: VRServer?) {
588588
skeleton.setLegTweaksEnabled(value)
589589
}
590590

591+
@VRServerThread
592+
fun setIKSolverEnabled(value: Boolean) {
593+
skeleton.setIKSolverEnabled(value)
594+
}
595+
591596
@VRServerThread
592597
fun setFloorClipEnabled(value: Boolean) {
593598
skeleton.setFloorclipEnabled(value)

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

Lines changed: 14 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -214,6 +214,7 @@ class HumanSkeleton(
214214
var legTweaks = LegTweaks(this)
215215
var tapDetectionManager = TapDetectionManager(this)
216216
var localizer = Localizer(this)
217+
var ikSolver = IKSolver(headBone)
217218

218219
// Stay Aligned
219220
var trackerSkeleton = TrackerSkeleton(this)
@@ -456,6 +457,9 @@ class HumanSkeleton(
456457
// Update tap detection's trackers
457458
tapDetectionManager.updateConfig(trackers)
458459

460+
// Rebuild Ik Solver
461+
ikSolver.buildChains(trackers)
462+
459463
// Update bones tracker field
460464
refreshBoneTracker()
461465

@@ -1173,7 +1177,7 @@ class HumanSkeleton(
11731177

11741178
SkeletonConfigToggles.SELF_LOCALIZATION -> localizer.setEnabled(newValue)
11751179

1176-
SkeletonConfigToggles.USE_POSITION -> newValue
1180+
SkeletonConfigToggles.USE_POSITION -> ikSolver.enabled = newValue
11771181

11781182
SkeletonConfigToggles.ENFORCE_CONSTRAINTS -> enforceConstraints = newValue
11791183

@@ -1543,6 +1547,7 @@ class HumanSkeleton(
15431547
}
15441548
legTweaks.resetBuffer()
15451549
localizer.reset()
1550+
ikSolver.resetOffsets()
15461551
LogManager.info("[HumanSkeleton] Reset: full ($resetSourceName)")
15471552
}
15481553

@@ -1695,6 +1700,14 @@ class HumanSkeleton(
16951700
legTweaks.enabled = value
16961701
}
16971702

1703+
/**
1704+
* enable/disable IK solver (for Autobone)
1705+
*/
1706+
@VRServerThread
1707+
fun setIKSolverEnabled(value: Boolean) {
1708+
ikSolver.enabled = value
1709+
}
1710+
16981711
@VRServerThread
16991712
fun setFloorclipEnabled(value: Boolean) {
17001713
humanPoseManager.setToggle(SkeletonConfigToggles.FLOOR_CLIP, value)

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

Lines changed: 159 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,159 @@
1+
package dev.slimevr.tracking.processor.skeleton
2+
3+
import dev.slimevr.tracking.processor.Bone
4+
import dev.slimevr.tracking.processor.Constraint.Companion.ConstraintType
5+
import dev.slimevr.tracking.trackers.Tracker
6+
import io.github.axisangles.ktmath.Quaternion
7+
import io.github.axisangles.ktmath.Vector3
8+
import kotlin.math.*
9+
10+
/*
11+
* This class implements a chain of Bones
12+
*/
13+
14+
class IKChain(
15+
val bones: MutableList<Bone>,
16+
var parent: IKChain?,
17+
val level: Int,
18+
val baseConstraint: Tracker?,
19+
val tailConstraint: Tracker?,
20+
) {
21+
// State variables
22+
private val computedBasePosition = baseConstraint?.let { IKConstraint(it) }
23+
private val computedTailPosition = tailConstraint?.let { IKConstraint(it) }
24+
var children = mutableListOf<IKChain>()
25+
var target = Vector3.NULL
26+
var distToTargetSqr = Float.POSITIVE_INFINITY
27+
private var rotations = getRotationsList()
28+
29+
private fun getRotationsList(): MutableList<Quaternion> {
30+
val rotList = mutableListOf<Quaternion>()
31+
for (b in bones) {
32+
rotList.add(b.getGlobalRotation())
33+
}
34+
35+
return rotList
36+
}
37+
38+
/*
39+
* Populate the non-static rotations with the solve angle from the last iteration
40+
*/
41+
private fun prepBones() {
42+
for (i in 0..<bones.size) {
43+
if (bones[i].rotationConstraint.constraintType != ConstraintType.COMPLETE) {
44+
bones[i].setRotationRaw(rotations[i])
45+
}
46+
}
47+
}
48+
49+
fun backwardsCCDIK() {
50+
target = computedTailPosition?.getPosition() ?: getChildTargetAvg()
51+
var offset = Vector3.NULL
52+
53+
for (i in bones.size - 1 downTo 0) {
54+
val currentBone = bones[i]
55+
56+
// Get the local position of the end effector and the target relative to the current node
57+
val endEffectorLocal = ((getEndEffectorsAvg() - offset) - currentBone.getPosition()).unit()
58+
val targetLocal = ((target - offset) - currentBone.getPosition()).unit()
59+
60+
// Compute the axis of rotation and angle for this bone
61+
var scalar = IKSolver.DAMPENING_FACTOR * if (currentBone.rotationConstraint.hasTrackerRotation) IKSolver.STATIC_DAMPENING else 1f
62+
scalar *= ((bones.size - i).toFloat() / bones.size).pow(IKSolver.ANNEALING_EXPONENT)
63+
val adjustment = Quaternion.fromTo(endEffectorLocal, targetLocal).pow(scalar).unit()
64+
65+
val rotation = currentBone.getGlobalRotation()
66+
var correctedRot = (adjustment * rotation).unit()
67+
68+
// Bones that are not supposed to be modified should tend towards their origin
69+
if (!currentBone.rotationConstraint.allowModifications) {
70+
correctedRot = correctedRot.interpR(currentBone.rotationConstraint.initialRotation, IKSolver.CORRECTION_FACTOR)
71+
}
72+
rotations[i] = setBoneRotation(currentBone, correctedRot)
73+
74+
if (currentBone.rotationConstraint.hasTrackerRotation) {
75+
offset += rotations[i].sandwich(Vector3.NEG_Y) * currentBone.length
76+
}
77+
}
78+
}
79+
80+
private fun getEndEffectorsAvg(): Vector3 {
81+
if (children.size < 1 || computedTailPosition != null) return bones.last().getTailPosition()
82+
83+
var sum = Vector3.NULL
84+
for (c in children) {
85+
sum += c.getEndEffectorsAvg()
86+
}
87+
88+
return sum / children.size.toFloat()
89+
}
90+
91+
private fun getChildTargetAvg(): Vector3 {
92+
if (computedTailPosition != null) return computedTailPosition.getPosition()
93+
94+
var sum = Vector3.NULL
95+
for (c in children) {
96+
sum += c.getChildTargetAvg()
97+
}
98+
99+
return sum / children.size.toFloat()
100+
}
101+
102+
/**
103+
* Resets the chain to its default state
104+
*/
105+
fun resetChain() {
106+
distToTargetSqr = Float.POSITIVE_INFINITY
107+
108+
for (b in bones) {
109+
b.rotationConstraint.initialRotation = b.getGlobalRotation()
110+
}
111+
prepBones()
112+
113+
for (child in children) {
114+
child.resetChain()
115+
}
116+
}
117+
118+
fun resetTrackerOffsets() {
119+
computedTailPosition?.reset(bones.last().getTailPosition())
120+
computedBasePosition?.reset(bones.first().getPosition())
121+
}
122+
123+
/**
124+
* Updates the distance to target and other fields
125+
* Call on the root chain
126+
*/
127+
fun computeTargetDistance() {
128+
distToTargetSqr = if (computedTailPosition != null) {
129+
(bones.last().getTailPosition() - computedTailPosition.getPosition()).lenSq()
130+
} else {
131+
0.0f
132+
}
133+
134+
for (chain in children) {
135+
chain.computeTargetDistance()
136+
}
137+
}
138+
139+
/**
140+
* Sets a bones rotation after constraining the rotation
141+
* to the bone's rotational constraint
142+
* returns the constrained rotation
143+
*/
144+
private fun setBoneRotation(bone: Bone, rotation: Quaternion): Quaternion {
145+
// Constrain relative to the parent
146+
val newRotation = if (bone.rotationConstraint.constraintType == ConstraintType.COMPLETE) {
147+
bone.rotationConstraint.applyConstraint(rotation, bone)
148+
} else if (!bone.rotationConstraint.hasTrackerRotation) {
149+
bone.rotationConstraint.applyConstraint(rotation, bone)
150+
} else {
151+
bone.rotationConstraint.constrainToInitialRotation(rotation)
152+
}
153+
154+
bone.setRotationRaw(newRotation)
155+
bone.update()
156+
157+
return newRotation
158+
}
159+
}

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

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
package dev.slimevr.tracking.processor.skeleton
2+
3+
import dev.slimevr.tracking.trackers.Tracker
4+
import io.github.axisangles.ktmath.Quaternion
5+
import io.github.axisangles.ktmath.Vector3
6+
import solarxr_protocol.datatypes.BodyPart
7+
8+
class IKConstraint(val tracker: Tracker) {
9+
private var offset = Vector3.NULL
10+
private var rotationOffset = Quaternion.IDENTITY
11+
12+
fun getPosition(): Vector3 =
13+
tracker.position + (tracker.getRotation() * rotationOffset).sandwich(offset)
14+
15+
fun reset(nodePosition: Vector3) {
16+
val bodyPartsToSkip = setOf(BodyPart.LEFT_HAND, BodyPart.RIGHT_HAND)
17+
18+
rotationOffset = tracker.getRotation().inv()
19+
if (tracker.trackerPosition?.bodyPart in bodyPartsToSkip) return
20+
offset = nodePosition - tracker.position
21+
}
22+
}

0 commit comments

Comments
 (0)