github/SlimeVR-Server
1
1
Fork 0
mirror of https://github.com/SlimeVR/SlimeVR-Server.git synced 2026-04-05 18:01:56 +02:00
Code Issues 156 Packages Projects Releases Wiki Activity

Toesnap and Footplant (#609)

Browse Source
This commit is contained in:
Collin Kees
2023-03-02 08:22:35 -08:00
committed by GitHub
parent 4ff29861aa
commit 2a65dc1086
9 changed files with 497 additions and 165 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
gui/public/i18n/en/translation.ftl
View File

@@ -272,8 +272,7 @@ settings-general-tracker_mechanics-drift_compensation-max_resets-label = Use up
## FK/Tracking settings
settings-general-fk_settings = Tracking settings
settings-general-fk_settings-leg_tweak = Leg tweaks
settings-general-fk_settings-leg_tweak-description = Floor-clip can Reduce or even eliminates clipping with the floor but may cause problems when on your knees. Skating-correction corrects for ice skating, but can decrease accuracy in certain movement patterns.
# Floor clip:
# why the name - came from the idea of noclip in video games, but is the opposite where clipping to the floor is a desired feature
# definition - Prevents the foot trackers from going lower than they where when a reset was performed
@@ -283,9 +282,16 @@ settings-general-fk_settings-leg_tweak-floor_clip = Floor clip
# since this largely prevents this it corrects for it hence skating correction (note this may be renamed to sliding correction)
# definition - Guesses when each foot is in contact with the ground and uses that information to improve tracking
settings-general-fk_settings-leg_tweak-skating_correction = Skating correction
settings-general-fk_settings-leg_tweak-toe_snap = Toe snap
settings-general-fk_settings-leg_tweak-foot_plant = Foot plant
settings-general-fk_settings-leg_tweak-skating_correction-amount = Skating correction strength
settings-general-fk_settings-leg_tweak-skating_correction-description = Skating-correction corrects for ice skating but can decrease accuracy in certain movement patterns. When enabling this make sure to full reset and recalibrate in game.
settings-general-fk_settings-leg_tweak-floor_clip-description = Floor-clip can Reduce or even eliminates clipping through the floor. When enabling this, make sure to full reset and recalibrate in game.
settings-general-fk_settings-leg_tweak-toe_snap-description = Toe-snap attempts to guess the rotation of your feet if feet trackers are not in use.
settings-general-fk_settings-leg_tweak-foot_plant-description = Foot-plant rotates your feet to be parallel to the ground when in contact.
settings-general-fk_settings-leg_fk = Leg tracking
settings-general-fk_settings-arm_fk = Arm tracking
settings-general-fk_settings-arm_fk-description = Change the way the arms are tracked.
settings-general-fk_settings-arm_fk-description = Force arms to be tracked from the HMD even if positional hand data is available.
settings-general-fk_settings-arm_fk-force_arms = Force arms from HMD
settings-general-fk_settings-skeleton_settings = Skeleton settings
settings-general-fk_settings-skeleton_settings-description = Toggle skeleton settings on or off. It is recommended to leave these on.

128
gui/src/components/settings/pages/GeneralSettings.tsx
View File

@@ -54,6 +54,8 @@ interface SettingsForm {
floorClip: boolean;
skatingCorrection: boolean;
viveEmulation: boolean;
toeSnap: boolean;
footPlant: boolean;
};
tapDetection: {
tapMountingResetEnabled: boolean;
@@ -92,6 +94,8 @@ const defaultValues = {
floorClip: false,
skatingCorrection: false,
viveEmulation: false,
toeSnap: false,
flootPlant: true,
},
filtering: { amount: 0.1, type: FilteringType.NONE },
driftCompensation: {
@@ -149,6 +153,8 @@ export function GeneralSettings() {
toggles.extendedSpine = values.toggles.extendedSpine;
toggles.forceArmsFromHmd = values.toggles.forceArmsFromHmd;
toggles.viveEmulation = values.toggles.viveEmulation;
toggles.toeSnap = values.toggles.toeSnap;
toggles.footPlant = values.toggles.footPlant;
legTweaks.correctionStrength = values.legTweaks.correctionStrength;
modelSettings.toggles = toggles;
@@ -485,26 +491,19 @@ export function GeneralSettings() {
<Typography variant="main-title">
{l10n.getString('settings-general-fk_settings')}
</Typography>
<Typography bold>
{l10n.getString('settings-general-fk_settings-leg_tweak')}
</Typography>
<div className="flex flex-col pt-2 pb-4">
<Typography bold>
{l10n.getString(
'settings-general-fk_settings-leg_tweak-skating_correction'
)}
</Typography>
<Typography color="secondary">
{l10n.getString(
'settings-general-fk_settings-leg_tweak-description'
'settings-general-fk_settings-leg_tweak-skating_correction-description'
)}
</Typography>
</div>
<div className="grid sm:grid-cols-2 gap-3 pb-5">
<CheckBox
variant="toggle"
outlined
control={control}
name="toggles.floorClip"
label={l10n.getString(
'settings-general-fk_settings-leg_tweak-floor_clip'
)}
/>
<div className="grid sm:grid-cols-1 gap-3 pb-4">
<CheckBox
variant="toggle"
outlined
@@ -514,8 +513,6 @@ export function GeneralSettings() {
'settings-general-fk_settings-leg_tweak-skating_correction'
)}
/>
</div>
<div className="flex sm:grid cols-1 gap3 pb-5">
<NumberSelector
control={control}
name="legTweaks.correctionStrength"
@@ -528,17 +525,78 @@ export function GeneralSettings() {
step={0.1}
/>
</div>
<Typography bold>
{l10n.getString('settings-general-fk_settings-arm_fk')}
</Typography>
<div className="flex flex-col pt-2 pb-4">
<div className="flex flex-col pt-2 pb-2">
<Typography bold>
{l10n.getString('settings-general-fk_settings-leg_fk')}
</Typography>
</div>
<div className="grid sm:grid-cols-1 gap-3 pb-3">
<Typography color="secondary">
{l10n.getString(
'settings-general-fk_settings-leg_tweak-floor_clip-description'
)}
</Typography>
</div>
<div className="grid sm:grid-cols-1 gap-3 pb-3">
<CheckBox
variant="toggle"
outlined
control={control}
name="toggles.floorClip"
label={l10n.getString(
'settings-general-fk_settings-leg_tweak-floor_clip'
)}
/>
</div>
<div className="flex flex-col pt-2 pb-3">
<Typography color="secondary">
{l10n.getString(
'settings-general-fk_settings-leg_tweak-foot_plant-description'
)}
</Typography>
</div>
<div className="grid sm:grid-cols-1 gap-3 pb-3">
<CheckBox
variant="toggle"
outlined
control={control}
name="toggles.footPlant"
label={l10n.getString(
'settings-general-fk_settings-leg_tweak-foot_plant'
)}
/>
</div>
<div className="flex flex-col pt-2 pb-3">
<Typography color="secondary">
{l10n.getString(
'settings-general-fk_settings-leg_tweak-toe_snap-description'
)}
</Typography>
</div>
<div className="grid sm:grid-cols-1 gap-3 pb-3">
<CheckBox
variant="toggle"
outlined
control={control}
name="toggles.toeSnap"
label={l10n.getString(
'settings-general-fk_settings-leg_tweak-toe_snap'
)}
/>
</div>
<div className="flex flex-col pt-2 pb-3">
<Typography bold>
{l10n.getString('settings-general-fk_settings-arm_fk')}
</Typography>
<Typography color="secondary">
{l10n.getString(
'settings-general-fk_settings-arm_fk-description'
)}
</Typography>
</div>
<div className="grid sm:grid-cols-2 pb-5">
<div className="grid sm:grid-cols-1 pb-3">
<CheckBox
variant="toggle"
outlined
@@ -551,19 +609,19 @@ export function GeneralSettings() {
</div>
{config?.debug && (
<>
<Typography bold>
{l10n.getString(
'settings-general-fk_settings-skeleton_settings'
)}
</Typography>
<div className="flex flex-col pt-2 pb-4">
<div className="flex flex-col pt-2 pb-3">
<Typography bold>
{l10n.getString(
'settings-general-fk_settings-skeleton_settings'
)}
</Typography>
<Typography color="secondary">
{l10n.getString(
'settings-general-fk_settings-skeleton_settings-description'
)}
</Typography>
</div>
<div className="grid sm:grid-cols-2 gap-3 pb-5">
<div className="grid sm:grid-cols-2 gap-3 pb-3">
<CheckBox
variant="toggle"
outlined
@@ -592,19 +650,19 @@ export function GeneralSettings() {
)}
/>
</div>
<Typography bold>
{l10n.getString(
'settings-general-fk_settings-vive_emulation-title'
)}
</Typography>
<div className="flex flex-col pt-2 pb-4">
<div className="flex flex-col pt-2 pb-3">
<Typography bold>
{l10n.getString(
'settings-general-fk_settings-vive_emulation-title'
)}
</Typography>
<Typography color="secondary">
{l10n.getString(
'settings-general-fk_settings-vive_emulation-description'
)}
</Typography>
</div>
<div className="grid sm:grid-cols-2 gap-3 pb-5">
<div className="grid sm:grid-cols-1 gap-3 pb-5">
<CheckBox
variant="toggle"
outlined

4
server/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsBuilder.java
View File

@@ -181,7 +181,9 @@ public class RPCSettingsBuilder {
humanPoseManager.getToggle(SkeletonConfigToggles.FORCE_ARMS_FROM_HMD),
humanPoseManager.getToggle(SkeletonConfigToggles.FLOOR_CLIP),
humanPoseManager.getToggle(SkeletonConfigToggles.SKATING_CORRECTION),
humanPoseManager.getToggle(SkeletonConfigToggles.VIVE_EMULATION)
humanPoseManager.getToggle(SkeletonConfigToggles.VIVE_EMULATION),
humanPoseManager.getToggle(SkeletonConfigToggles.TOE_SNAP),
humanPoseManager.getToggle(SkeletonConfigToggles.FOOT_PLANT)
);
int ratiosOffset = ModelRatios
.createModelRatios(

2
server/src/main/java/dev/slimevr/protocol/rpc/settings/RPCSettingsHandler.java
View File

@@ -299,6 +299,8 @@ public record RPCSettingsHandler(RPCHandler rpcHandler, ProtocolAPI api) {
toggles.skatingCorrection()
);
hpm.setToggle(SkeletonConfigToggles.VIVE_EMULATION, toggles.viveEmulation());
hpm.setToggle(SkeletonConfigToggles.TOE_SNAP, toggles.toeSnap());
hpm.setToggle(SkeletonConfigToggles.FOOT_PLANT, toggles.footPlant());
}
if (ratios != null) {

4
server/src/main/java/dev/slimevr/tracking/processor/config/SkeletonConfigToggles.java
View File

@@ -12,7 +12,9 @@ public enum SkeletonConfigToggles {
FORCE_ARMS_FROM_HMD(4, "Force arms from HMD", "forceArmsFromHMD", true),
FLOOR_CLIP(5, "Floor clip", "floorClip", true),
SKATING_CORRECTION(6, "Skating correction", "skatingCorrection", true),
VIVE_EMULATION(7, "Vive emulation", "viveEmulation", false),;
VIVE_EMULATION(7, "Vive emulation", "viveEmulation", false),
TOE_SNAP(8, "Toe Snap", "toeSnap", false),
FOOT_PLANT(9, "Foot Plant", "footPlant", true),;
public static final SkeletonConfigToggles[] values = values();
private static final Map<String, SkeletonConfigToggles> byStringVal = new HashMap<>();

2
server/src/main/java/dev/slimevr/tracking/processor/skeleton/HumanSkeleton.java
View File

@@ -1239,6 +1239,8 @@ public class HumanSkeleton {
case SKATING_CORRECTION -> legTweaks.setSkatingReductionEnabled(newValue);
case FLOOR_CLIP -> legTweaks.setFloorclipEnabled(newValue);
case VIVE_EMULATION -> viveEmulation.setEnabled(newValue);
case TOE_SNAP -> legTweaks.setToeSnap(newValue);
case FOOT_PLANT -> legTweaks.setFootPlant(newValue);
}
}

123
server/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweakBuffer.java
View File

@@ -6,16 +6,16 @@ import com.jme3.math.FastMath;
/**
* class that holds data related to the state and other variuse attributes of
* class that holds data related to the state and other various attributes of
* the legs such as the position of the foot, knee, and waist, after and before
* correction, the velocity of the foot and the computed state of the feet at
* that frame. mainly calculates the state of the legs per frame using these
* rules: The conditions for an unlock are as follows: 1. the foot is to far
* from its correct position 2. a velocity higher than a threashold is achived
* rules: The conditions for an unlock are as follows: 1. the foot is too far
* from its correct position 2. a velocity higher than a threshold is achieved
* 3. a large acceleration is applied to the foot 4. angular velocity of the
* foot goes higher than a threshold. The conditions for a lock are the opposite
* of the above but require a lower value for all of the above conditions. The
* afformentioned thresholds are computed by applying scalers to a base
* aforementioned thresholds are computed by applying scalars to a base
* threshold value. This allows one set of initial values to be applicable to a
* large range of actions and body types.
*/
@@ -50,6 +50,8 @@ public class LegTweakBuffer {
private Vector3f leftKneePositionCorrected = new Vector3f();
private Vector3f rightKneePositionCorrected = new Vector3f();
private Vector3f waistPositionCorrected = new Vector3f();
private Quaternion leftFootRotationCorrected = new Quaternion();
private Quaternion rightFootRotationCorrected = new Quaternion();
// velocities
private Vector3f leftFootVelocity = new Vector3f();
@@ -80,15 +82,15 @@ public class LegTweakBuffer {
// hyperparameters
public static final float SKATING_DISTANCE_CUTOFF = 0.5f;
static float SKATING_VELOCITY_THRESHOLD = 2.6f;
static float SKATING_ACCELERATION_THRESHOLD = 0.8f;
static float SKATING_VELOCITY_THRESHOLD = 2.4f;
static float SKATING_ACCELERATION_THRESHOLD = 0.7f;
private static final float SKATING_ROTVELOCITY_THRESHOLD = 4.5f;
private static final float SKATING_LOCK_ENGAGE_PERCENT = 0.85f;
private static final float SKATING_ACCELERATION_Y_USE_PERCENT = 0.25f;
private static final float FLOOR_DISTANCE_CUTOFF = 0.125f;
private static final float SIX_TRACKER_TOLLERANCE = -0.10f;
private static final float SIX_TRACKER_TOLERANCE = -0.10f;
private static final Vector3f FORCE_VECTOR_TO_PRESSURE = new Vector3f(0.25f, 1.0f, 0.25f);
private static final float FORCE_ERROR_TOLLERANCE = 4.0f;
private static final float FORCE_ERROR_TOLERANCE_SQR = FastMath.sqr(4.0f);
private static final float[] FORCE_VECTOR_FALLBACK = new float[] { 0.1f, 0.1f };
static float PARAM_SCALAR_MAX = 3.2f;
@@ -109,9 +111,9 @@ public class LegTweakBuffer {
private static final float MIN_SCALAR_ACTIVE = 1.75f;
private static final float MAX_SCALAR_ACTIVE = 0.1f;
// maximum scalers for the pressure on each foot
private static final float PRESSURE_SCALER_MIN = 0.1f;
private static final float PRESSURE_SCALER_MAX = 1.9f;
// maximum scalars for the pressure on each foot
private static final float PRESSURE_SCALAR_MIN = 0.1f;
private static final float PRESSURE_SCALAR_MAX = 1.9f;
private float leftFootSensitivityVel = 1.0f;
private float rightFootSensitivityVel = 1.0f;
@@ -204,6 +206,28 @@ public class LegTweakBuffer {
this.rightFootRotation.set(rightFootRotation);
}
public Quaternion getLeftFootRotationCorrected(Quaternion quat) {
if (quat == null)
quat = new Quaternion();
return quat.set(leftFootRotationCorrected);
}
public void setLeftFootRotationCorrected(Quaternion quat) {
this.leftFootRotationCorrected.set(quat);
}
public Quaternion getRightFootRotationCorrected(Quaternion quat) {
if (quat == null)
quat = new Quaternion();
return quat.set(rightFootRotationCorrected);
}
public void setRightFootRotationCorrected(Quaternion quat) {
this.rightFootRotationCorrected.set(quat);
}
public Vector3f getLeftFootPositionCorrected(Vector3f vec) {
if (vec == null)
vec = new Vector3f();
@@ -374,14 +398,14 @@ public class LegTweakBuffer {
this.detectionMode = mode;
}
// calculate momvent attributes
// calculate movement attributes
public void calculateFootAttributes(boolean active) {
updateFrameNumber(0);
// compute attributes of the legs
computeVelocity();
computeAccelerationMagnitude();
computeComAtributes();
computeComAttributes();
// check if the acceleration triggers forced unlock
if (detectionMode == FOOT_ACCEL) {
@@ -424,9 +448,10 @@ public class LegTweakBuffer {
// check if a locked foot should stay locked or be released
private int checkStateLeft() {
float timeStep = getTimeDelta();
if (parent.leftLegState == UNLOCKED) {
if (
parent.getLeftFootHorizantalDifference() > SKATING_CUTOFF_ENGAGE
parent.getLeftFootHorizontalDifference() > SKATING_CUTOFF_ENGAGE
|| leftFootVelocityMagnitude * timeStep
> SKATING_VELOCITY_CUTOFF_ENGAGE * leftFootSensitivityVel
|| leftFootAngleDiff * timeStep
@@ -441,7 +466,7 @@ public class LegTweakBuffer {
}
if (
parent.getLeftFootHorizantalDifference() > SKATING_DISTANCE_CUTOFF
parent.getLeftFootHorizontalDifference() > SKATING_DISTANCE_CUTOFF
|| leftFootVelocityMagnitude * timeStep
> SKATING_VELOCITY_THRESHOLD * leftFootSensitivityVel
|| leftFootAngleDiff * timeStep
@@ -461,11 +486,11 @@ public class LegTweakBuffer {
if (parent.rightLegState == UNLOCKED) {
if (
parent.getRightFootHorizantalDifference() > SKATING_CUTOFF_ENGAGE
parent.getRightFootHorizontalDifference() > SKATING_CUTOFF_ENGAGE
|| rightFootVelocityMagnitude * timeStep
> SKATING_VELOCITY_CUTOFF_ENGAGE * leftFootSensitivityVel
> SKATING_VELOCITY_CUTOFF_ENGAGE * rightFootSensitivityVel
|| rightFootAngleDiff * timeStep
> SKATING_ROTATIONAL_VELOCITY_CUTOFF_ENGAGE * leftFootSensitivityVel
> SKATING_ROTATIONAL_VELOCITY_CUTOFF_ENGAGE * rightFootSensitivityVel
|| rightFootPosition.y > rightFloorLevel + FLOOR_DISTANCE_CUTOFF
|| accelerationAboveThresholdRight
) {
@@ -476,7 +501,7 @@ public class LegTweakBuffer {
}
if (
parent.getRightFootHorizantalDifference() > SKATING_DISTANCE_CUTOFF
parent.getRightFootHorizontalDifference() > SKATING_DISTANCE_CUTOFF
|| rightFootVelocityMagnitude * timeStep
> SKATING_VELOCITY_THRESHOLD * rightFootSensitivityVel
|| rightFootAngleDiff * timeStep
@@ -491,14 +516,14 @@ public class LegTweakBuffer {
}
// get the difference in feet position between the kinematic and corrected
// positions of the feet disregarding vertical displacment
private float getLeftFootHorizantalDifference() {
// positions of the feet disregarding vertical displacement
private float getLeftFootHorizontalDifference() {
return leftFootPositionCorrected.subtract(leftFootPosition).setY(0).length();
}
// get the difference in feet position between the kinematic and corrected
// positions of the feet
private float getRightFootHorizantalDifference() {
private float getRightFootHorizontalDifference() {
return rightFootPositionCorrected.subtract(rightFootPosition).setY(0).length();
}
@@ -550,7 +575,7 @@ public class LegTweakBuffer {
}
// compute the velocity and acceleration of the center of mass
private void computeComAtributes() {
private void computeComAttributes() {
centerOfMassVelocity = centerOfMass.subtract(parent.centerOfMass);
centerOfMassAcceleration = centerOfMassVelocity.subtract(parent.centerOfMassVelocity);
}
@@ -567,9 +592,9 @@ public class LegTweakBuffer {
// determine lock/unlock
private void computeAccelerationAboveThresholdAnkleTrackers() {
accelerationAboveThresholdLeft = leftFootAccelerationMagnitude
> (SKATING_ACCELERATION_THRESHOLD + SIX_TRACKER_TOLLERANCE) * leftFootSensitivityAccel;
> (SKATING_ACCELERATION_THRESHOLD + SIX_TRACKER_TOLERANCE) * leftFootSensitivityAccel;
accelerationAboveThresholdRight = rightFootAccelerationMagnitude
> (SKATING_ACCELERATION_THRESHOLD + SIX_TRACKER_TOLLERANCE) * rightFootSensitivityAccel;
> (SKATING_ACCELERATION_THRESHOLD + SIX_TRACKER_TOLERANCE) * rightFootSensitivityAccel;
}
// using the parent lock/unlock states, velocity, and acceleration,
@@ -581,12 +606,12 @@ public class LegTweakBuffer {
float leftFootScalarAccel = getLeftFootScalarAccel();
float rightFootScalarAccel = getRightFootScalarAccel();
// get the second set of scalars that is based of of how close each foot
// is to a lock and dynamically adjusting the scalars
// (based off the assumption that if you are standing one foot is likly
// get the second set of scalars that is based off of how close each
// foot is to a lock and dynamically adjusting the scalars
// (based off the assumption that if you are standing one foot is likely
// planted on the ground unless you are moving fast)
float leftFootScalarVel = getLeftFootLockLiklyHood();
float rightFootScalarVel = getRightFootLockLiklyHood();
float leftFootScalarVel = getLeftFootLockLikelihood();
float rightFootScalarVel = getRightFootLockLikelihood();
// get the third set of scalars that is based on where the COM is
float[] pressureScalars = getPressurePrediction();
@@ -594,10 +619,10 @@ public class LegTweakBuffer {
// combine the scalars to get the final scalars
leftFootSensitivityVel = (leftFootScalarAccel
+ leftFootScalarVel / 2.0f)
* FastMath.clamp(pressureScalars[0] * 2.0f, PRESSURE_SCALER_MIN, PRESSURE_SCALER_MAX);
* FastMath.clamp(pressureScalars[0] * 2.0f, PRESSURE_SCALAR_MIN, PRESSURE_SCALAR_MAX);
rightFootSensitivityVel = (rightFootScalarAccel
+ rightFootScalarVel / 2.0f)
* FastMath.clamp(pressureScalars[1] * 2.0f, PRESSURE_SCALER_MIN, PRESSURE_SCALER_MAX);
* FastMath.clamp(pressureScalars[1] * 2.0f, PRESSURE_SCALAR_MIN, PRESSURE_SCALAR_MAX);
leftFootSensitivityAccel = leftFootScalarVel;
rightFootSensitivityAccel = rightFootScalarVel;
@@ -638,7 +663,7 @@ public class LegTweakBuffer {
// states to calculate a scalar to apply to the non acceleration based
// hyperparameters when calculating
// lock states
private float getLeftFootLockLiklyHood() {
private float getLeftFootLockLikelihood() {
if (leftLegState == LOCKED && rightLegState == LOCKED) {
Vector3f velocityDiff = leftFootVelocity.subtract(rightFootVelocity);
velocityDiff.setY(0.0f);
@@ -654,24 +679,24 @@ public class LegTweakBuffer {
}
// calculate the 'unlockedness factor' and use that to
// determine the scalar (go as low as 0.5 as as high as
// determine the scalar (go as low as 0.5 and as high as
// param_scalar_max)
float velocityDifAbs = Math.abs(leftFootVelocityMagnitude)
- Math.abs(rightFootVelocityMagnitude);
float velocityDiffAbs = FastMath
.abs(leftFootVelocityMagnitude - rightFootVelocityMagnitude);
if (velocityDifAbs > MIN_SCALAR_ACTIVE) {
if (velocityDiffAbs > MIN_SCALAR_ACTIVE) {
return PARAM_SCALAR_MIN;
} else if (velocityDifAbs < MAX_SCALAR_ACTIVE) {
} else if (velocityDiffAbs < MAX_SCALAR_ACTIVE) {
return PARAM_SCALAR_MAX;
}
return PARAM_SCALAR_MAX
* (velocityDifAbs - MIN_SCALAR_ACTIVE)
* (velocityDiffAbs - MIN_SCALAR_ACTIVE)
/ (MAX_SCALAR_ACTIVE - MIN_SCALAR_ACTIVE)
- PARAM_SCALAR_MID;
}
private float getRightFootLockLiklyHood() {
private float getRightFootLockLikelihood() {
if (rightLegState == LOCKED && leftLegState == LOCKED) {
Vector3f velocityDiff = rightFootVelocity.subtract(leftFootVelocity);
velocityDiff.setY(0.0f);
@@ -687,19 +712,19 @@ public class LegTweakBuffer {
}
// calculate the 'unlockedness factor' and use that to
// determine the scalar (go as low as 0.5 as as high as
// determine the scalar (go as low as 0.5 and as high as
// param_scalar_max)
float velocityDifAbs = Math.abs(rightFootVelocityMagnitude)
- Math.abs(leftFootVelocityMagnitude);
float velocityDiffAbs = FastMath
.abs(rightFootVelocityMagnitude - leftFootVelocityMagnitude);
if (velocityDifAbs > MIN_SCALAR_ACTIVE) {
if (velocityDiffAbs > MIN_SCALAR_ACTIVE) {
return PARAM_SCALAR_MIN;
} else if (velocityDifAbs < MAX_SCALAR_ACTIVE) {
} else if (velocityDiffAbs < MAX_SCALAR_ACTIVE) {
return PARAM_SCALAR_MAX;
}
return PARAM_SCALAR_MAX
* (velocityDifAbs - MIN_SCALAR_ACTIVE)
* (velocityDiffAbs - MIN_SCALAR_ACTIVE)
/ (MAX_SCALAR_ACTIVE - MIN_SCALAR_ACTIVE)
- PARAM_SCALAR_MID;
}
@@ -768,7 +793,7 @@ public class LegTweakBuffer {
private void findForceVectors(Vector3f leftFootForce, Vector3f rightFootForce) {
int iterations = 100;
float stepSize = 0.01f;
// setup the temporary variables
// set up the temporary variables
Vector3f tempLeftFootForce1 = leftFootForce.clone();
Vector3f tempLeftFootForce2 = leftFootForce.clone();
Vector3f tempRightFootForce1 = rightFootForce.clone();
@@ -817,11 +842,11 @@ public class LegTweakBuffer {
}
// detect any outside forces on the body such
// as a wall or a chair. returns true if there is a outside force
// as a wall or a chair. returns true if there is an outside force
private boolean detectOutsideForces(Vector3f f1, Vector3f f2) {
Vector3f force = GRAVITY.add(f1).add(f2);
Vector3f error = centerOfMassAcceleration.subtract(force);
return error.lengthSquared() > FastMath.sqr(FORCE_ERROR_TOLLERANCE);
return error.lengthSquared() > FORCE_ERROR_TOLERANCE_SQR;
}
// simple error function for the force vector gradient descent

385
server/src/main/java/dev/slimevr/tracking/processor/skeleton/LegTweaks.java
View File

@@ -5,21 +5,91 @@ import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import dev.slimevr.config.LegTweaksConfig;
import dev.slimevr.tracking.processor.TransformNode;
import dev.slimevr.tracking.processor.config.SkeletonConfigToggles;
public class LegTweaks {
/**
* here is an explanation of each parameter that may need explaining
* STANDING_CUTOFF_VERTICAL is the percentage the waist has to be below its
* position at calibration to register as the user not standing
* MAX_DISENGAGEMENT_OFFSET is how much the floor will be shifted to allow
* an offset to happen smoothly DYNAMIC_DISPLACEMENT_CUTOFF is the percent
* of downwards rotation that can contribute to dynamic displacement
* MAX_DYNAMIC_DISPLACEMENT is the max amount the floor will be moved up to
* account for the foot rotating downward and needing to be put higher to
* avoid clipping in the game world MIN_ACCEPTABLE_ERROR and
* MAX_ACCEPTABLE_ERROR Defines the distance where CORRECTION_WEIGHT_MIN and
* CORRECTION_WEIGHT_MAX are calculating a percent of velocity to correct
* rather than using the min or max FLOOR_CALIBRATION_OFFSET is the amount
* the floor plane is shifted up. This can help the feet from floating
* slightly above the ground
*/
// hyperparameters (clip correction)
static float DYNAMIC_DISPLACEMENT_CUTOFF = 1.0f;
private static final float FLOOR_CALIBRATION_OFFSET = 0.015f;
// hyperparameters (skating correction)
private static final float MIN_ACCEPTABLE_ERROR = 0.01f;
private static final float MAX_ACCEPTABLE_ERROR = 0.05f;
private static final float CORRECTION_WEIGHT_MIN = 0.55f;
private static final float CORRECTION_WEIGHT_MAX = 0.70f;
private static final float CONTINUOUS_CORRECTION_DIST = 0.5f;
private static final int CONTINUOUS_CORRECTION_WARMUP = 175;
// hyperparameters (knee / waist correction)
private static final float KNEE_CORRECTION_WEIGHT = 0.00f;
private static final float KNEE_LATERAL_WEIGHT = 0.8f;
private static final float WAIST_PUSH_WEIGHT = 0.2f;
// hyperparameters (COM calculation)
// mass percentages of the body
private static final float HEAD_MASS = 0.082f;
private static final float CHEST_MASS = 0.25f;
private static final float WAIST_MASS = 0.209f;
private static final float THIGH_MASS = 0.128f;
private static final float CALF_MASS = 0.0535f;
private static final float UPPER_ARM_MASS = 0.031f;
private static final float FOREARM_MASS = 0.017f;
// hyperparameters (rotation correction)
private static final float ROTATION_CORRECTION_VERTICAL = 0.1f;
private static final float MAXIMUM_CORRECTION_ANGLE = 0.4f;
private static final float MAXIMUM_CORRECTION_ANGLE_DELTA = 0.7f;
private static final float MAXIMUM_TOE_DOWN_ANGLE = 0.8f;
private static final float TOE_SNAP_COOLDOWN = 3.0f;
// hyperparameters (misc)
static final float NEARLY_ZERO = 0.001f;
private static final float STANDING_CUTOFF_VERTICAL = 0.65f;
private static final float MAX_DISENGAGEMENT_OFFSET = 0.30f;
private static final float DEFAULT_ARM_DISTANCE = 0.15f;
private static final float MAX_CORRECTION_STRENGTH_DELTA = 1.0f;
// state variables
private float floorLevel;
private float waistToFloorDist;
private float currentDisengagementOffset = 0.0f;
private float footLength = 0.0f;
private static float currentCorrectionStrength = 0.3f; // default value
private boolean initialized = true;
private boolean enabled = true; // master switch
private boolean floorclipEnabled = false;
private boolean skatingCorrectionEnabled = false;
private boolean toeSnap = false;
private boolean footPlant = false;
private boolean active = false;
private boolean rightLegActive = false;
private boolean leftLegActive = false;
private int leftFramesLocked = 0;
private int rightFramesLocked = 0;
private int leftFramesUnlocked = 0;
private int rightFramesUnlocked = 0;
private float leftToeAngle = 0.0f;
private boolean leftToeTouched = false;
private float rightToeAngle = 0.0f;
private boolean rightToeTouched = false;
// skeleton and config
private HumanSkeleton skeleton;
@@ -44,65 +114,7 @@ public class LegTweaks {
private Vector3f rightKneePlaceholder = new Vector3f();
private boolean kneesActive = false;
/**
* here is a explination of each parameter that may need explaining
* STANDING_CUTOFF_VERTICAL is the percentage the waist has to be below its
* position at calibration to register as the user not standing
* MAX_DISENGAGMENT_OFFSET is how much the floor will be shifted to allow an
* offset to happen smoothly DYNAMIC_DISPLACEMENT_CUTOFF is the percent of
* downwards rotation that can contribute to dynamic displacment
* MAX_DYNAMIC_DISPLACMENT is the max amount the floor will be moved up to
* account for the foot rotating downward and needing to be put higher to
* avoid clipping in the gameworld MIN_ACCEPTABLE_ERROR and
* MAX_ACCEPTABLE_ERROR Defines the disitance where CORRECTION_WEIGHT_MIN
* and CORRECTION_WEIGHT_MAX are calculating a percent of velocity to
* correct rather than using the min or max FLOOR_CALIBRATION_OFFSET is the
* amount the floor plane is shifted up. This can help the feet from
* floating slightly above the ground
*/
// hyperparameters (clip correction)
static float DYNAMIC_DISPLACEMENT_CUTOFF = 1.0f;
static float MAX_DYNAMIC_DISPLACEMENT = 0.06f;
private static final float FLOOR_CALIBRATION_OFFSET = 0.015f;
// hyperparameters (skating correction)
private static final float MIN_ACCEPTABLE_ERROR = 0.01f;
private static final float MAX_ACCEPTABLE_ERROR = 0.225f;
private static final float CORRECTION_WEIGHT_MIN = 0.40f;
private static final float CORRECTION_WEIGHT_MAX = 0.70f;
private static final float CONTINUOUS_CORRECTION_DIST = 0.5f;
private static final int CONTINUOUS_CORRECTION_WARMUP = 175;
// hyperparameters (knee / waist correction)
private static final float KNEE_CORRECTION_WEIGHT = 0.00f;
private static final float KNEE_LATERAL_WEIGHT = 0.8f;
private static final float WAIST_PUSH_WEIGHT = 0.2f;
// hyperparameters (COM calculation)
// mass percentages of the body
private static final float HEAD_MASS = 0.082f;
private static final float CHEST_MASS = 0.25f;
private static final float WAIST_MASS = 0.209f;
private static final float THIGH_MASS = 0.128f;
private static final float CALF_MASS = 0.0535f;
private static final float UPPER_ARM_MASS = 0.031f;
private static final float FOREARM_MASS = 0.017f;
// hyperparameters (misc)
static final float NEARLY_ZERO = 0.001f;
private static final float STANDING_CUTOFF_VERTICAL = 0.65f;
private static final float MAX_DISENGAGMENT_OFFSET = 0.30f;
private static final float DEFAULT_ARM_DISTANCE = 0.15f;
private static final float MAX_CORRECTION_STRENGTH_DELTA = 1.0f;
// counters
private int leftFramesLocked = 0;
private int rightFramesLocked = 0;
private int leftFramesUnlocked = 0;
private int rightFramesUnlocked = 0;
// buffer for holding previus frames of data
// buffer for holding previous frames of data
private LegTweakBuffer bufferHead = new LegTweakBuffer();
private boolean bufferInvalid = true;
@@ -201,6 +213,14 @@ public class LegTweaks {
this.bufferInvalid = true;
}
public void setToeSnap(boolean val) {
this.toeSnap = val;
}
public void setFootPlant(boolean val) {
this.footPlant = val;
}
public boolean getEnabled() {
return this.enabled;
}
@@ -213,6 +233,14 @@ public class LegTweaks {
return this.skatingCorrectionEnabled;
}
public boolean getToeSnap() {
return this.toeSnap;
}
public boolean getFootPlant() {
return this.footPlant;
}
public void resetBuffer() {
bufferInvalid = true;
}
@@ -224,9 +252,15 @@ public class LegTweaks {
public void updateConfig() {
LegTweaks.updateHyperParameters(config.getCorrectionStrength());
floorclipEnabled = skeleton.humanPoseManager.getToggle(SkeletonConfigToggles.FLOOR_CLIP);
skatingCorrectionEnabled = skeleton.humanPoseManager
.getToggle(SkeletonConfigToggles.SKATING_CORRECTION);
toeSnap = skeleton.humanPoseManager.getToggle(SkeletonConfigToggles.TOE_SNAP);
footPlant = skeleton.humanPoseManager.getToggle(SkeletonConfigToggles.FOOT_PLANT);
}
// update the hyper parameters with the config
// update the hyperparameters with the config
public static void updateHyperParameters(float newStrength) {
LegTweakBuffer.SKATING_VELOCITY_THRESHOLD = getScaledHyperParameter(
newStrength,
@@ -248,7 +282,7 @@ public class LegTweaks {
private void setVectors() {
// set the positions of the feet and knees to the skeletons current
// positions
if (skeleton.computedLeftKneeTracker != null || skeleton.computedRightKneeTracker != null) {
if (skeleton.computedLeftKneeTracker != null && skeleton.computedRightKneeTracker != null) {
kneesActive = true;
leftKneePosition = skeleton.computedLeftKneeTracker.position;
rightKneePosition = skeleton.computedRightKneeTracker.position;
@@ -300,13 +334,16 @@ public class LegTweaks {
floorLevel = (leftFootPosition.y + rightFootPosition.y) / 2f + FLOOR_CALIBRATION_OFFSET;
waistToFloorDist = waistPosition.y - floorLevel;
// invalidate the buffer since the non initialized output may be
// invalidate the buffer since the non-initialized output may be
// very wrong
bufferInvalid = true;
initialized = true;
}
// if not enabled do nothing and return false
// update the foot length
footLength = skeleton.leftFootNode.localTransform.getTranslation().length();
// if not enabled, do nothing and return false
if (!enabled)
return false;
@@ -329,7 +366,7 @@ public class LegTweaks {
bufferHead.setLeftLegState(LegTweakBuffer.UNLOCKED);
bufferHead.setRightLegState(LegTweakBuffer.UNLOCKED);
// if the system is active propulate the buffer with corrected floor
// if the system is active, populate the buffer with corrected floor
// clip feet positions
if (active && isStanding()) {
correctClipping();
@@ -353,19 +390,19 @@ public class LegTweaks {
currentFrame
.setLeftFloorLevel(
(floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getLeftFootOffset()))
(floorLevel + (footLength * getLeftFootOffset()))
- currentDisengagementOffset
);
currentFrame
.setRightFloorLevel(
(floorLevel + (MAX_DYNAMIC_DISPLACEMENT * getRightFootOffset()))
(floorLevel + (footLength * getRightFootOffset()))
- currentDisengagementOffset
);
// put the acceleration vector that is applicable to the tracker
// quantity in the the buffer
// (if feet are not available, fallback to 6 tracker mode)
// quantity in the buffer
// (if feet are not available, fall back to 6 tracker mode)
if (skeleton.leftFootTracker != null && skeleton.rightFootTracker != null) {
currentFrame.setLeftFootAcceleration(leftFootAcceleration);
currentFrame.setRightFootAcceleration(rightFootAcceleration);
@@ -395,6 +432,9 @@ public class LegTweaks {
if (!preUpdate())
return;
// correct foot rotation's
correctFootRotations();
// push the feet up if needed
if (floorclipEnabled)
correctClipping();
@@ -476,13 +516,13 @@ public class LegTweaks {
// move the feet to their new positions
if (
leftFootPosition.y
< (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * leftOffset))
< (floorLevel + (footLength * leftOffset))
- currentDisengagementOffset
) {
float displacement = Math
.abs(
floorLevel
+ (MAX_DYNAMIC_DISPLACEMENT * leftOffset)
+ (footLength * leftOffset)
- leftFootPosition.y
- currentDisengagementOffset
);
@@ -494,13 +534,13 @@ public class LegTweaks {
if (
rightFootPosition.y
< (floorLevel + (MAX_DYNAMIC_DISPLACEMENT * rightOffset))
< (floorLevel + (footLength * rightOffset))
- currentDisengagementOffset
) {
float displacement = Math
.abs(
floorLevel
+ (MAX_DYNAMIC_DISPLACEMENT * rightOffset)
+ (footLength * rightOffset)
- rightFootPosition.y
- currentDisengagementOffset
);
@@ -713,6 +753,176 @@ public class LegTweaks {
}
}
// correct the rotations of the feet
// this is done by planting the foot better and by snapping the toes to the
// ground
private void correctFootRotations() {
// null check's
if (bufferHead == null || bufferHead.getParent() == null)
return;
// if there is a foot tracker for a foot don't correct it
if (skeleton.leftFootTracker != null || skeleton.rightFootTracker != null)
return;
// get the foot positions
Quaternion leftFootRotation = bufferHead.getLeftFootRotation(null);
Quaternion rightFootRotation = bufferHead.getRightFootRotation(null);
// between maximum correction angle and maximum correction angle delta
// the values are interpolated
float kneeAngleL = getXZAmount(leftFootPosition, leftKneePosition);
float kneeAngleR = getXZAmount(rightFootPosition, rightKneePosition);
float masterWeightL = getMasterWeight(kneeAngleL);
float masterWeightR = getMasterWeight(kneeAngleR);
// corrects rotations when planted firmly on the ground
if (footPlant) {
// prepare the weight vars for this correction step
float weightL = 0.0f;
float weightR = 0.0f;
// the further from the ground the foot is, the less weight it
// should have
weightL = getFootPlantWeight(leftFootPosition);
weightR = getFootPlantWeight(rightFootPosition);
// perform the correction
leftFootRotation
.set(
leftFootRotation
.slerp(
leftFootRotation,
isolateYaw(leftFootRotation),
weightL * masterWeightL
)
);
rightFootRotation
.set(
rightFootRotation
.slerp(
rightFootRotation,
isolateYaw(rightFootRotation),
weightR * masterWeightR
)
);
}
// corrects rotations when the foot is in the air by rotating the foot
// down so that the toes are touching
if (toeSnap) {
// this correction step has its own weight vars
float weightL = 0.0f;
float weightR = 0.0f;
// first compute the angle of the foot
float angleL = getToeSnapAngle(leftFootPosition);
float angleR = getToeSnapAngle(rightFootPosition);
// then compute the weight of the correction
weightL = getToeSnapWeight(leftFootPosition, angleL);
weightR = getToeSnapWeight(rightFootPosition, angleR);
// depending on the state variables, the correction weights should
// be clamped
if (!leftToeTouched) {
weightL = Math.min(weightL, leftToeAngle);
}
if (!rightToeTouched) {
weightR = Math.min(weightR, rightToeAngle);
}
// then slerp the rotation to the new rotation based on the weight
leftFootRotation
.slerp(
leftFootRotation,
replacePitch(leftFootRotation, -angleL),
weightL * masterWeightL
);
rightFootRotation
.slerp(
rightFootRotation,
replacePitch(rightFootRotation, -angleR),
weightR * masterWeightR
);
// update state variables regarding toe snap
if (leftFootPosition.y - floorLevel > footLength * MAXIMUM_TOE_DOWN_ANGLE) {
leftToeTouched = false;
leftToeAngle = weightL;
} else if (leftFootPosition.y - floorLevel < 0.0f) {
leftToeTouched = true;
leftToeAngle = 1.0f;
}
if (rightFootPosition.y - floorLevel > footLength * MAXIMUM_TOE_DOWN_ANGLE) {
rightToeTouched = false;
rightToeAngle = weightR;
} else if (rightFootPosition.y - floorLevel < 0.0f) {
rightToeTouched = true;
rightToeAngle = 1.0f;
}
}
// update the foot rotations in the buffer
bufferHead.setLeftFootRotationCorrected(leftFootRotation);
bufferHead.setRightFootRotationCorrected(rightFootRotation);
// finally update the skeletons rotations with the new rotations
skeleton.computedLeftFootTracker.rotation.set(leftFootRotation);
skeleton.computedRightFootTracker.rotation.set(rightFootRotation);
}
// returns the length of the xz components of the normalized difference
// between two vectors
public float getXZAmount(Vector3f vec1, Vector3f vec2) {
return vec1
.subtract(vec2)
.normalizeLocal()
.setY(0.0f)
.length();
}
// returns a float between 0 and 1 that represents the master weight for
// foot rotation correciton
private float getMasterWeight(float kneeAngle) {
float masterWeight = (kneeAngle > MAXIMUM_CORRECTION_ANGLE
&& kneeAngle < MAXIMUM_CORRECTION_ANGLE_DELTA)
? 1.0f
- ((kneeAngle - MAXIMUM_CORRECTION_ANGLE)
/ (MAXIMUM_CORRECTION_ANGLE_DELTA - MAXIMUM_CORRECTION_ANGLE))
: 0.0f;
return (kneeAngle < MAXIMUM_CORRECTION_ANGLE) ? 1.0f : masterWeight;
}
// return the weight of the correction for toe snap
private float getToeSnapWeight(Vector3f footPos, float angle) {
// then compute the weight of the correction
float weight = ((footPos.y - floorLevel) > footLength * TOE_SNAP_COOLDOWN)
? 0.0f
: 1.0f
- ((footPos.y - floorLevel - footLength)
/ (footLength * (TOE_SNAP_COOLDOWN - 1.0f)));
return FastMath.clamp(weight, 0.0f, 1.0f);
}
// returns the angle of the foot for toe snap
private float getToeSnapAngle(Vector3f footPos) {
float angle = FastMath.clamp(footPos.y - floorLevel, 0.0f, footLength);
return (angle > footLength * MAXIMUM_TOE_DOWN_ANGLE)
? FastMath.asin((footLength * MAXIMUM_TOE_DOWN_ANGLE) / footLength)
: FastMath.asin((angle / footLength));
}
// returns the weight for floor plant
private float getFootPlantWeight(Vector3f footPos) {
float weight = (footPos.y - floorLevel > ROTATION_CORRECTION_VERTICAL)
? 0.0f
: 1.0f - ((footPos.y - floorLevel) / ROTATION_CORRECTION_VERTICAL);
return FastMath.clamp(weight, 0.0f, 1.0f);
}
// returns true if it is likely the user is standing
public boolean isStanding() {
// if the waist is below the vertical cutoff, user is not standing
@@ -722,7 +932,7 @@ public class LegTweaks {
if (waistPosition.y < cutoff) {
currentDisengagementOffset = (1 - waistPosition.y / cutoff)
* MAX_DISENGAGMENT_OFFSET;
* MAX_DISENGAGEMENT_OFFSET;
return false;
}
@@ -741,7 +951,7 @@ public class LegTweaks {
Vector3f tempLeft;
Vector3f tempRight;
// before moveing the knees back closer to the waist nodes offset them
// before moving the knees back closer to the waist nodes, offset them
// the same amount the foot trackers where offset
float leftXDif = leftFootPosition.x - bufferHead.getLeftFootPosition(null).x;
float rightXDif = rightFootPosition.x - bufferHead.getRightFootPosition(null).x;
@@ -869,7 +1079,7 @@ public class LegTweaks {
centerOfMass = centerOfMass.add(leftForearm.mult(FOREARM_MASS));
centerOfMass = centerOfMass.add(rightForearm.mult(FOREARM_MASS));
} else {
// if the arms are not avaliable put them slightly in front
// if the arms are not available put them slightly in front
// of the chest.
Vector3f chestUnitVector = computeUnitVector(
skeleton.chestNode.worldTransform.getRotation()
@@ -932,6 +1142,31 @@ public class LegTweaks {
}
}
// remove the x and z components of the given quaternion
private Quaternion isolateYaw(Quaternion quaternion) {
return new Quaternion(
0,
quaternion.getY(),
0,
quaternion.getW()
);
}
// return a quaternion that has been rotated by the new pitch amount
private Quaternion replacePitch(Quaternion quaternion, float newPitch) {
// first get the axis of the current pitch
Vector3f currentPitchAxis = quaternion.getRotationColumn(0).normalize();
// then get the current pitch
float currentPitch = (float) Math.asin(currentPitchAxis.y);
// then add the new pitch
Quaternion newQuat = new Quaternion();
newQuat.fromAngleAxis(newPitch + currentPitch, currentPitchAxis);
return newQuat.mult(quaternion);
}
// check if the difference between two floats flipped after correction
private boolean checkOverShoot(float trueVal, float valBefore, float valAfter) {
return (trueVal - valBefore) * (trueVal - valAfter) < 0;

2
solarxr-protocol

Submodule solarxr-protocol updated: 378ca6e236...05647cfad0