Extended knee model + temporary fix for null/optional (#193)

This commit is contained in:
Erimel authored and GitHub committed 2022-06-09 13:21:40 +03:00
1 parent db33c561ab
commit c02b95346f
10 files changed
+134 -100

No files matched your search

@@ -654,7 +654,11 @@ public class AutoBone {
List<PoseFrameTracker> trackers = frames.getTrackers();
for (PoseFrameTracker tracker : trackers) {
TrackerFrame trackerFrame = tracker.safeGetFrame(cursor);
if (trackerFrame == null || !trackerFrame.hasData(TrackerFrameData.POSITION)) {
if (
trackerFrame == null
|| !trackerFrame.hasData(TrackerFrameData.POSITION)
|| trackerFrame.designation.trackerRole.isEmpty()
) {
continue;
}
@@ -689,7 +693,11 @@ public class AutoBone {
}
TrackerFrame trackerFrame2 = tracker.safeGetFrame(cursor2);
if (trackerFrame2 == null || !trackerFrame2.hasData(TrackerFrameData.POSITION)) {
if (
trackerFrame2 == null
|| !trackerFrame2.hasData(TrackerFrameData.POSITION)
|| trackerFrame1.designation.trackerRole.isEmpty()
) {
continue;
}
@@ -699,6 +707,10 @@ public class AutoBone {
continue;
}
if (trackerFrame2.designation.trackerRole.isEmpty()) {
continue;
}
Vector3f nodePos2 = skeleton2
.getComputedTracker(trackerFrame2.designation.trackerRole.get()).position;
if (nodePos2 == null) {
@@ -185,9 +185,11 @@ public class TrackersList extends EJBoxNoStretch {
desSelect.addItem(p.name());
}
if (cfg.designation != null) {
TrackerPosition p = TrackerPosition.getByDesignation(cfg.designation).get();
if (p != null)
desSelect.setSelectedItem(p.name());
TrackerPosition
.getByDesignation(cfg.designation)
.ifPresent(
trackerPosition -> desSelect.setSelectedItem(trackerPosition.name())
);
}
desSelect.addActionListener(new ActionListener() {
@Override
@@ -114,7 +114,7 @@ public class WindowsNamedPipeBridge extends ProtobufBridge<VRTracker> implements
);
TrackerRole role = TrackerRole.getById(trackerAdded.getTrackerRole());
if (role != null) {
tracker.setBodyPosition(TrackerPosition.getByTrackerRole(role).get());
TrackerPosition.getByTrackerRole(role).ifPresent(tracker::setBodyPosition);
}
return tracker;
}
@@ -92,7 +92,9 @@ public final class PoseFrameIO {
TrackerPosition designation = null;
if (TrackerFrameData.DESIGNATION.check(dataFlags)) {
designation = TrackerPosition.getByDesignation(inputStream.readUTF()).get();
designation = TrackerPosition
.getByDesignation(inputStream.readUTF())
.orElse(null);
}
Quaternion rotation = null;
@@ -3,10 +3,9 @@ package dev.slimevr.protocol;
import com.fazecast.jSerialComm.SerialPort;
import com.google.flatbuffers.FlatBufferBuilder;
import com.jme3.math.Quaternion;
import dev.slimevr.autobone.AutoBone.Epoch;
import dev.slimevr.autobone.AutoBoneListener;
import dev.slimevr.autobone.AutoBoneProcessType;
import dev.slimevr.autobone.AutoBone.Epoch;
import dev.slimevr.platform.windows.WindowsNamedPipeBridge;
import dev.slimevr.poserecorder.PoseFrames;
import dev.slimevr.serial.SerialListener;
@@ -197,7 +196,7 @@ public class RPCHandler extends ProtocolHandler<RpcMessageHeader>
if (tracker == null)
return;
tracker.setBodyPosition(TrackerPosition.getByBodyPart(req.bodyPosition()).get());
TrackerPosition.getByBodyPart(req.bodyPosition()).ifPresent(tracker::setBodyPosition);
if (tracker instanceof ReferenceAdjustedTracker) {
ReferenceAdjustedTracker refTracker = (ReferenceAdjustedTracker) tracker;
@@ -1,11 +1,7 @@
package dev.slimevr.vr.processor;
import dev.slimevr.vr.trackers.ComputedTracker;
import dev.slimevr.vr.trackers.ShareableTracker;
import dev.slimevr.vr.trackers.TrackerRole;
import dev.slimevr.vr.trackers.TrackerWithTPS;
import dev.slimevr.vr.trackers.*;
import dev.slimevr.vr.trackers.udp.UDPDevice;
import dev.slimevr.vr.trackers.TrackerPosition;
import io.eiren.util.BufferedTimer;
@@ -25,7 +21,7 @@ public class ComputedHumanPoseTracker extends ComputedTracker
this.skeletonPosition = skeletonPosition;
this.trackerRole = role;
// TODO: Use `TrackerPosition` instead of `TrackerRole`
this.bodyPosition = TrackerPosition.getByTrackerRole(role).get();
this.bodyPosition = TrackerPosition.getByTrackerRole(role).orElse(null);
}
@Override
@@ -129,8 +129,6 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
private final Quaternion rotBuf2 = new Quaternion();
private final Quaternion rotBuf3 = new Quaternion();
private final Quaternion rotBuf4 = new Quaternion();
protected final Vector3f vectorBuf1 = new Vector3f();
protected final Vector3f vectorBuf2 = new Vector3f();
protected boolean hasSpineTracker;
protected boolean hasKneeTrackers;
protected float minKneePitch = 0f * FastMath.DEG_TO_RAD;
@@ -173,17 +171,19 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
// #endregion
// #region FK Settings
protected boolean extendedPelvisModel = true;
protected boolean extendedSpineModel = true;
protected boolean extendedKneeModel = false;
protected boolean extendedPelvisModel = true;
protected boolean extendedKneeModel = true;
// Extended Spine Model
protected float waistChestHipFactor = 0.5f;
protected float waistChestPelvisFactor = 0.18f;
protected float hipSpinePelvisFactor = 0.25f;
protected float pelvisHipFactor = FastMath.ONE_THIRD;
protected float waistChestHipAveraging = 0.5f;
protected float waistChestPelvisAveraging = 0.18f;
protected float hipSpinePelvisAveraging = 0.25f;
protected float pelvisHipAveraging = FastMath.ONE_THIRD;
// Extended Pelvis Model
protected float pelvisWaistTrackerFactor = 0.75f;
protected float pelvisWaistTrackerAveraging = 0.75f;
// Extended Knee Model
protected float ankleKneeTrackerAveraging = 0.25f;
// #endregion
// #region Constructors
@@ -252,8 +252,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
// #endregion
// Set default skeleton configuration (callback automatically sets
// initial
// offsets)
// initial offsets)
skeletonConfig = new SkeletonConfig(true, this);
if (computedTrackers != null) {
@@ -859,17 +858,9 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
if (leftLowerLegTracker != null)
leftLowerLegTracker.getRotation(rotBuf2);
if (leftUpperLegTracker != null && leftLowerLegTracker != null && extendedKneeModel)
calculateKneeLimits(
rotBuf1,
rotBuf2,
leftUpperLegTracker.getConfidenceLevel(),
leftLowerLegTracker.getConfidenceLevel()
);
leftHipNode.localTransform.setRotation(rotBuf1);
trackerLeftKneeNode.localTransform.setRotation(rotBuf1);
leftKneeNode.localTransform.setRotation(rotBuf2);
trackerLeftKneeNode.localTransform.setRotation(rotBuf2);
if (leftFootTracker != null)
leftFootTracker.getRotation(rotBuf2);
@@ -878,6 +869,32 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
leftFootNode.localTransform.setRotation(rotBuf2);
trackerLeftFootNode.localTransform.setRotation(rotBuf2);
// Extended left knee
if (leftUpperLegTracker != null && leftLowerLegTracker != null && extendedKneeModel) {
// Averages the knee's rotation with the local ankle's
// pitch and roll and apply to the tracker node.
leftKneeNode.localTransform.getRotation(rotBuf1);
leftHipNode.localTransform.getRotation(rotBuf2);
// Get the knee's inverse rotation.
rotBuf3.set(rotBuf2);
rotBuf3.inverseLocal();
// Only rotate on local yaw and pitch
//
// R = InverseKnee * Ankle
// C = Quaternion(-R.x, 0, 0, R.w)
// Knee = Knee * R * C
// normalize(Knee)
rotBuf3.multLocal(rotBuf1);
rotBuf1.set(-rotBuf3.getX(), 0, 0, rotBuf3.getW());
rotBuf1.set(rotBuf2.mult(rotBuf3).mult(rotBuf1));
rotBuf1.normalizeLocal();
rotBuf1.slerpLocal(rotBuf2, ankleKneeTrackerAveraging);
trackerLeftKneeNode.localTransform.setRotation(rotBuf1);
}
// Right Leg
rotBuf1.loadIdentity();
rotBuf2.loadIdentity();
@@ -886,17 +903,9 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
if (rightLowerLegTracker != null)
rightLowerLegTracker.getRotation(rotBuf2);
if (rightUpperLegTracker != null && rightLowerLegTracker != null && extendedKneeModel)
calculateKneeLimits(
rotBuf1,
rotBuf2,
rightUpperLegTracker.getConfidenceLevel(),
rightLowerLegTracker.getConfidenceLevel()
);
rightHipNode.localTransform.setRotation(rotBuf1);
trackerRightKneeNode.localTransform.setRotation(rotBuf1);
rightKneeNode.localTransform.setRotation(rotBuf2);
trackerRightKneeNode.localTransform.setRotation(rotBuf2);
if (rightFootTracker != null)
rightFootTracker.getRotation(rotBuf2);
@@ -905,6 +914,33 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
rightFootNode.localTransform.setRotation(rotBuf2);
trackerRightFootNode.localTransform.setRotation(rotBuf2);
// Extended right knee
if (rightUpperLegTracker != null && rightLowerLegTracker != null && extendedKneeModel) {
// Averages the knee's rotation with the local ankle's
// pitch and roll and apply to the tracker node.
rightKneeNode.localTransform.getRotation(rotBuf1);
rightHipNode.localTransform.getRotation(rotBuf2);
// Get the knee's inverse rotation.
rotBuf3.set(rotBuf2);
rotBuf3.inverseLocal();
// Only rotate on local yaw and pitch
//
// R = InverseKnee * Ankle
// C = Quaternion(-R.x, 0, 0, R.w)
// Knee = Knee * R * C
// normalize(Knee)
rotBuf3.multLocal(rotBuf1);
rotBuf1.set(-rotBuf3.getX(), 0, 0, rotBuf3.getW());
rotBuf1.set(rotBuf2.mult(rotBuf3).mult(rotBuf1));
rotBuf1.normalizeLocal();
rotBuf1.slerpLocal(rotBuf2, ankleKneeTrackerAveraging);
trackerRightKneeNode.localTransform.setRotation(rotBuf1);
}
// Extended spine
if (extendedSpineModel && hasSpineTracker) {
if (
@@ -919,7 +955,8 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
chestTracker.getRotation(rotBuf1);
hipTracker.getRotation(rotBuf2);
rotBuf1.slerpLocal(rotBuf2, waistChestHipFactor);
// Interpolate between the chest and the hip
rotBuf1.slerpLocal(rotBuf2, waistChestHipAveraging);
chestNode.localTransform.setRotation(rotBuf1);
} else if (hasKneeTrackers) {
// Calculates waist from chest + pelvis
@@ -937,8 +974,12 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
rotBuf2.negateLocal();
}
// Average the legs to calculate the pelvis
rotBuf1.nlerp(rotBuf2, 0.5f);
rotBuf3.pureSlerpLocal(rotBuf1, waistChestPelvisFactor);
// Interpolate between the pelvis and the chest
rotBuf3.pureSlerpLocal(rotBuf1, waistChestPelvisAveraging);
chestNode.localTransform.setRotation(rotBuf3);
}
}
@@ -950,8 +991,8 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
.getFirstAvailableTracker(waistTracker, chestTracker, null)
.getRotation(rotBuf3);
// Get the rotation relative to where we expect the upper
// legs to be
// Get the rotation relative to where we expect the
// upper legs to be
rotBuf3.mult(FORWARD_QUATERNION, rotBuf4);
if (rotBuf4.dot(rotBuf1) < 0.0f) {
rotBuf1.negateLocal();
@@ -960,8 +1001,12 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
rotBuf2.negateLocal();
}
// Average the legs to calculate the pelvis
rotBuf1.nlerp(rotBuf2, 0.5f);
rotBuf3.pureSlerpLocal(rotBuf1, hipSpinePelvisFactor);
// Interpolate between the pelvis and the chest
rotBuf3.pureSlerpLocal(rotBuf1, hipSpinePelvisAveraging);
waistNode.localTransform.setRotation(rotBuf3);
}
}
@@ -975,12 +1020,11 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
rotBuf2.nlerp(rotBuf1, 0.5f);
waistNode.localTransform.getRotation(rotBuf1);
rotBuf2.slerpLocal(rotBuf1, pelvisHipFactor);
rotBuf2.slerpLocal(rotBuf1, pelvisHipAveraging);
hipNode.localTransform.setRotation(rotBuf2);
// Averages the trackerWaistNode's rotation with the calculated
// pelvis'
// on local yaw and roll. git blame AxisAngle :p
// pelvis' on local yaw and roll. git blame AxisAngle :p
leftHipNode.localTransform.getRotation(rotBuf1);
rightHipNode.localTransform.getRotation(rotBuf2);
waistNode.localTransform.getRotation(rotBuf3);
@@ -994,17 +1038,22 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
rotBuf2.negateLocal();
}
// Get WaiswaistNode's inverse rotation.
// Get waistNode's inverse rotation.
rotBuf4.set(rotBuf3);
rotBuf4.inverseLocal();
// Only rotate on local yaw and pitch
rotBuf4.set(rotBuf4.mult(rotBuf1.add(rotBuf2)));
//
// R = InverseWaist * (LeftLeft + RightLeg)
// C = Quaternion(-R.x, 0, 0, R.w)
// Pelvis = Waist * R * C
// normalize(Pelvis)
rotBuf4.multLocal(rotBuf1.add(rotBuf2));
rotBuf2.set(-rotBuf4.getX(), 0, 0, rotBuf4.getW());
rotBuf1.set(rotBuf3.mult(rotBuf4).mult(rotBuf2));
rotBuf1.normalizeLocal();
rotBuf1.slerpLocal(rotBuf3, pelvisWaistTrackerFactor);
rotBuf1.slerpLocal(rotBuf3, pelvisWaistTrackerAveraging);
trackerWaistNode.localTransform.setRotation(rotBuf1);
}
@@ -1099,37 +1148,6 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
}
}
// #region Knee Model
// Knee basically has only 1 DoF (pitch), average yaw and roll between knee
// and hip
protected void calculateKneeLimits(
Quaternion hipBuf,
Quaternion kneeBuf,
float hipConfidence,
float kneeConfidence
) {
vectorBuf1.set(0, -1, 0);
vectorBuf2.set(0, -1, 0);
hipBuf.multLocal(vectorBuf1);
kneeBuf.multLocal(vectorBuf2);
// Find knee angle
kneeRotation.angleBetweenVectors(vectorBuf1, vectorBuf2);
// Substract knee angle from knee rotation. With perfect leg and perfect
// sensors result should match hip rotation perfectly
kneeBuf.multLocal(kneeRotation.inverse());
// Average knee and hip
hipBuf.nlerp(kneeBuf, 0.5f);
// TODO : Use confidence to calculate changeAmt
kneeBuf.set(hipBuf);
// Return knee angle into knee rotation
kneeBuf.multLocal(kneeRotation);
}
// #endregion
// #region Update the output trackers
protected void updateComputedTrackers() {
if (computedChestTracker != null) {
@@ -1147,7 +1165,7 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
if (computedLeftKneeTracker != null) {
computedLeftKneeTracker.position
.set(trackerLeftKneeNode.worldTransform.getTranslation());
computedLeftKneeTracker.rotation.set(leftHipNode.worldTransform.getRotation());
computedLeftKneeTracker.rotation.set(trackerLeftKneeNode.worldTransform.getRotation());
computedLeftKneeTracker.dataTick();
}
@@ -1161,7 +1179,8 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
if (computedRightKneeTracker != null) {
computedRightKneeTracker.position
.set(trackerRightKneeNode.worldTransform.getTranslation());
computedRightKneeTracker.rotation.set(rightHipNode.worldTransform.getRotation());
computedRightKneeTracker.rotation
.set(trackerRightKneeNode.worldTransform.getRotation());
computedRightKneeTracker.dataTick();
}
@@ -1236,12 +1255,12 @@ public class HumanSkeleton extends Skeleton implements SkeletonConfigCallback {
// Cache the values of these configs
switch (configToggle) {
case EXTENDED_PELVIS_MODEL:
extendedPelvisModel = newValue;
break;
case EXTENDED_SPINE_MODEL:
extendedSpineModel = newValue;
break;
case EXTENDED_PELVIS_MODEL:
extendedPelvisModel = newValue;
break;
case EXTENDED_KNEE_MODEL:
extendedKneeModel = newValue;
break;
@@ -6,12 +6,12 @@ import java.util.Map;
public enum SkeletonConfigToggle {
EXTENDED_PELVIS_MODEL("Extended pelvis model", "extendedPelvis", true),
EXTENDED_SPINE_MODEL("Extended spine model", "extendedSpine", true),
EXTENDED_KNEE_MODEL("Extended knee model", "extendedKnee", false),;
EXTENDED_SPINE_MODEL("Extended spine model", "spine", true),
EXTENDED_PELVIS_MODEL("Extended pelvis model", "pelvis", true),
EXTENDED_KNEE_MODEL("Extended knee model", "knee", true),;
public static final SkeletonConfigToggle[] values = values();
private static final String CONFIG_PREFIX = "body.model.";
private static final String CONFIG_PREFIX = "body.extendedModel.";
private static final Map<String, SkeletonConfigToggle> byStringVal = new HashMap<>();
static {
@@ -46,7 +46,9 @@ public class ComputedTracker implements Tracker, TrackerWithTPS {
// not be
// allowed if editing is not allowed
if (userEditable()) {
bodyPosition = TrackerPosition.getByDesignation(config.designation).get();
TrackerPosition
.getByDesignation(config.designation)
.ifPresent(trackerPosition -> bodyPosition = trackerPosition);
}
}
@@ -4,8 +4,8 @@ import com.jme3.math.FastMath;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import dev.slimevr.VRServer;
import dev.slimevr.vr.trackers.udp.UDPDevice;
import dev.slimevr.vr.trackers.udp.TrackersUDPServer;
import dev.slimevr.vr.trackers.udp.UDPDevice;
import io.eiren.util.BufferedTimer;
@@ -83,7 +83,9 @@ public class IMUTracker implements Tracker, TrackerWithTPS, TrackerWithBattery {
} else {
rotAdjust.loadIdentity();
}
bodyPosition = TrackerPosition.getByDesignation(config.designation).get();
TrackerPosition
.getByDesignation(config.designation)
.ifPresent(trackerPosition -> bodyPosition = trackerPosition);
setFilter(
vrserver.config.getString("filters.type"),
vrserver.config.getFloat("filters.amount", 0.3f),