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

Added basic skeleton proportions settings GUI

Browse Source 
Added chest tracker support
Fixed full adjusted trackers
This commit is contained in:
Eiren Rain
2021-05-22 17:20:55 +03:00
parent 48cfeec677
commit c5ffe0c2e8
11 changed files with 226 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
src/main/java/io/eiren/gui/SkeletonConfig.java Normal file
View File

@@ -0,0 +1,86 @@
package io.eiren.gui;
import java.awt.event.MouseEvent;
import java.util.HashMap;
import java.util.Map;
import javax.swing.JButton;
import javax.swing.JLabel;
import javax.swing.event.MouseInputAdapter;
import io.eiren.util.StringUtils;
import io.eiren.util.ann.ThreadSafe;
import io.eiren.vr.VRServer;
import io.eiren.vr.processor.HumanSkeleton;
public class SkeletonConfig extends EJBag {
private final VRServer server;
private final VRServerGUI gui;
private Map<String, SkeletonLabel> labels = new HashMap<>();
public SkeletonConfig(VRServer server, VRServerGUI gui) {
super();
this.server = server;
this.gui = gui;
setAlignmentY(TOP_ALIGNMENT);
server.humanPoseProcessor.addSkeletonUpdatedCallback(this::skeletonUpdated);
}
@ThreadSafe
public void skeletonUpdated(HumanSkeleton newSkeleton) {
java.awt.EventQueue.invokeLater(() -> {
removeAll();
int row = 0;
add(new JLabel("Waist"), c(0, row, 1));
add(new AdjButton("+", "Waist", 0.01f), c(1, row, 1));
add(new SkeletonLabel("Waist"), c(2, row, 1));
add(new AdjButton("-", "Waist", -0.01f), c(3, row, 1));
row++;
add(new JLabel("Virtual waist"), c(0, row, 1));
add(new AdjButton("+", "Virtual waist", 0.01f), c(1, row, 1));
add(new SkeletonLabel("Virtual waist"), c(2, row, 1));
add(new AdjButton("-", "Virtual waist", -0.01f), c(3, row, 1));
row++;
add(new JLabel("Head shift"), c(0, row, 1));
add(new AdjButton("+", "Head", 0.01f), c(1, row, 1));
add(new SkeletonLabel("Head"), c(2, row, 1));
add(new AdjButton("-", "Head", -0.01f), c(3, row, 1));
row++;
gui.refresh();
});
}
private void change(String joint, float diff) {
float current = server.humanPoseProcessor.getSkeletonConfig(joint);
server.humanPoseProcessor.setSkeletonConfig(joint, current + diff);
labels.get(joint).setText(StringUtils.prettyNumber(current + diff, 2));
}
private class SkeletonLabel extends JLabel {
public SkeletonLabel(String joint) {
super(StringUtils.prettyNumber(server.humanPoseProcessor.getSkeletonConfig(joint), 2));
labels.put(joint, this);
}
}
private class AdjButton extends JButton {
public AdjButton(String text, String joint, float diff) {
super(text);
addMouseListener(new MouseInputAdapter() {
@Override
public void mouseClicked(MouseEvent e) {
change(joint, diff);
}
});
}
}
}

4
src/main/java/io/eiren/gui/TrackersList.java
View File

@@ -88,7 +88,7 @@ public class TrackersList extends EJBag {
if(cfg.designation != null)
add(new JLabel(cfg.designation), c(1, n, 2));
if(t instanceof CalibratingTracker) {
/*if(t instanceof CalibratingTracker) {
add(new JButton("Calibrate") {{
addMouseListener(new MouseInputAdapter() {
@Override
@@ -97,7 +97,7 @@ public class TrackersList extends EJBag {
}
});
}}, c(12, n, 2));
}
}*/
n += tr.getSize();
}
gui.refresh();

2
src/main/java/io/eiren/gui/VRServerGUI.java
View File

@@ -81,6 +81,8 @@ public class VRServerGUI extends JFrame {
setAlignmentY(TOP_ALIGNMENT);
add(new JLabel("Skeleton"));
add(skeletonList);
add(new JLabel("Skeleton config"));
add(new SkeletonConfig(server, VRServerGUI.this));
add(Box.createVerticalGlue());
}});
}});

1
src/main/java/io/eiren/vr/processor/ComputedHumanPoseTrackerPosition.java
View File

@@ -3,6 +3,7 @@ package io.eiren.vr.processor;
public enum ComputedHumanPoseTrackerPosition {
WAIST,
CHEST,
LEFT_ANKLE,
RIGHT_ANKLE;
}

47
src/main/java/io/eiren/vr/processor/HumanPoseProcessor.java
View File

@@ -12,8 +12,8 @@ import io.eiren.util.ann.ThreadSafe;
import io.eiren.util.ann.VRServerThread;
import io.eiren.util.collections.FastList;
import io.eiren.vr.VRServer;
import io.eiren.vr.trackers.AdjustedFullTracker;
import io.eiren.vr.trackers.AdjustedTracker;
import io.eiren.vr.trackers.AdjustedYawTracker;
import io.eiren.vr.trackers.HMDTracker;
import io.eiren.vr.trackers.Tracker;
import io.eiren.vr.trackers.TrackerConfig;
@@ -43,6 +43,19 @@ public class HumanPoseProcessor {
consumer.accept(skeleton);
}
@ThreadSafe
public void setSkeletonConfig(String key, float newLength) {
if(skeleton != null)
skeleton.setSkeletonConfig(key, newLength);
}
@ThreadSafe
public float getSkeletonConfig(String key) {
if(skeleton != null)
return skeleton.getSkeletonConfig().get(key);
return 0.0f;
}
@ThreadSafe
public List<? extends Tracker> getComputedTrackers() {
return computedTrackers;
@@ -61,7 +74,7 @@ public class HumanPoseProcessor {
@VRServerThread
private void addTracker(Tracker tracker, TrackerBodyPosition position) {
AdjustedTracker tt = new AdjustedYawTracker(tracker);
AdjustedTracker tt = new AdjustedFullTracker(tracker);
trackers.put(position, tt);
server.registerTracker(tt);
@@ -72,30 +85,36 @@ public class HumanPoseProcessor {
private void updateSekeltonModel() {
boolean hasWaist = false;
boolean hasBothLegs = false;
//boolean hasChest = false;
boolean hasChest = false;
if(trackers.get(TrackerBodyPosition.WAIST) != null)
hasWaist = true;
//if(trackers.get(TrackerBodyPosition.CHEST) != null)
// hasChest = true;
if(trackers.get(TrackerBodyPosition.CHEST) != null)
hasChest = true;
if(trackers.get(TrackerBodyPosition.LEFT_ANKLE) != null && trackers.get(TrackerBodyPosition.LEFT_LEG) != null
&& trackers.get(TrackerBodyPosition.RIGHT_ANKLE) != null && trackers.get(TrackerBodyPosition.RIGHT_LEG) != null)
hasBothLegs = true;
if(!hasWaist) {
if(!hasWaist && !hasChest) {
skeleton = null; // Can't track anything without waist
} else if(hasBothLegs) {
if(skeleton instanceof HumanSekeletonWithLegs) {
return; // Proper skeleton applied
}
disconnectAllTrackers();
skeleton = new HumanSekeletonWithLegs(server, trackers, computedTrackers);
AdjustedTracker waist = trackers.get(TrackerBodyPosition.WAIST);
if(waist == null)
waist = trackers.get(TrackerBodyPosition.CHEST);
AdjustedTracker chest = trackers.get(TrackerBodyPosition.CHEST);
if(chest == null)
chest = trackers.get(TrackerBodyPosition.WAIST);
skeleton = new HumanSekeletonWithLegs(server, waist, chest, trackers, computedTrackers);
for(int i = 0; i < onSkeletonUpdated.size(); ++i)
onSkeletonUpdated.get(i).accept(skeleton);
} else {
if(skeleton instanceof HumanSkeleonWithWaist) {
return; // Proper skeleton applied
}
AdjustedTracker waist = trackers.get(TrackerBodyPosition.WAIST);
if(waist == null)
waist = trackers.get(TrackerBodyPosition.CHEST);
AdjustedTracker chest = trackers.get(TrackerBodyPosition.CHEST);
if(chest == null)
chest = trackers.get(TrackerBodyPosition.WAIST);
disconnectAllTrackers();
skeleton = new HumanSkeleonWithWaist(server, trackers.get(TrackerBodyPosition.WAIST), computedTrackers);
skeleton = new HumanSkeleonWithWaist(server, waist, chest, computedTrackers);
for(int i = 0; i < onSkeletonUpdated.size(); ++i)
onSkeletonUpdated.get(i).accept(skeleton);
}

10
src/main/java/io/eiren/vr/processor/HumanSekeletonWithLegs.java
View File

@@ -34,23 +34,23 @@ public class HumanSekeletonWithLegs extends HumanSkeleonWithWaist {
/**
* Distance between centers of both hips
*/
protected float hipsWidth = 0.33f;
protected float hipsWidth = 0.30f;
/**
* Length from waist to knees
*/
protected float hipsLength = 0.46f;
protected float hipsLength = 0.51f;
/**
* Distance from waist to ankle
*/
protected float ankleLength = 0.5f;
protected float ankleLength = 0.55f;
protected float minKneePitch = 0f * FastMath.DEG_TO_RAD;
protected float maxKneePitch = 90f * FastMath.DEG_TO_RAD;
protected float kneeLerpFactor = 0.5f;
public HumanSekeletonWithLegs(VRServer server, Map<TrackerBodyPosition, ? extends Tracker> trackers, List<ComputedHumanPoseTracker> computedTrackers) {
super(server, trackers.get(TrackerBodyPosition.WAIST), computedTrackers);
public HumanSekeletonWithLegs(VRServer server, Tracker waistTracker, Tracker chestTracker, Map<TrackerBodyPosition, ? extends Tracker> trackers, List<ComputedHumanPoseTracker> computedTrackers) {
super(server, waistTracker, chestTracker, computedTrackers);
this.leftLegTracker = trackers.get(TrackerBodyPosition.LEFT_LEG);
this.leftAnkleTracker = trackers.get(TrackerBodyPosition.LEFT_ANKLE);
this.rightLegTracker = trackers.get(TrackerBodyPosition.RIGHT_LEG);

44
src/main/java/io/eiren/vr/processor/HumanSkeleonWithWaist.java
View File

@@ -22,25 +22,27 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
protected final Quaternion qBuf = new Quaternion();
protected final Vector3f vBuf = new Vector3f();
protected final Tracker wasitTracker;
protected final Tracker waistTracker;
protected final Tracker chestTracker;
protected final HMDTracker hmdTracker;
protected final ComputedHumanPoseTracker computedWaistTracker;
protected final TransformNode hmdNode = new TransformNode("HMD", false);
protected final TransformNode headNode = new TransformNode("Head", false);
protected final TransformNode neckNode = new TransformNode("Neck", false);
protected final TransformNode waistNode = new TransformNode("Waist", false);
protected final TransformNode chestNode = new TransformNode("Chest", false);
protected final TransformNode trackerWaistNode = new TransformNode("Waist-Tracker", false);
/**
* Distance from eyes to waist
*/
protected float waistDistance = 0.55f;
protected float waistDistance = 0.85f;
/**
* Distance from eyes to waist, defines reported
* tracker position, if you want to move resulting
* tracker up or down from actual waist
*/
protected float trackerWaistDistance = 0.55f;
protected float trackerWaistDistance = 0.57f;
/**
* Distacne from eyes to the base of the neck
*/
@@ -50,8 +52,9 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
*/
protected float headShift = 0.1f;
public HumanSkeleonWithWaist(VRServer server, Tracker waistTracker, List<ComputedHumanPoseTracker> computedTrackers) {
this.wasitTracker = waistTracker;
public HumanSkeleonWithWaist(VRServer server, Tracker waistTracker, Tracker chestTracker, List<ComputedHumanPoseTracker> computedTrackers) {
this.waistTracker = waistTracker;
this.chestTracker = chestTracker;
this.hmdTracker = server.hmdTracker;
this.server = server;
ComputedHumanPoseTracker cwt = null;
@@ -73,11 +76,14 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
headNode.attachChild(neckNode);
neckNode.localTransform.setTranslation(0, -neckLength, 0);
neckNode.attachChild(waistNode);
waistNode.localTransform.setTranslation(0, -waistDistance, 0);
neckNode.attachChild(chestNode);
chestNode.localTransform.setTranslation(0, -waistDistance / 2, 0);
neckNode.attachChild(trackerWaistNode);
trackerWaistNode.localTransform.setTranslation(0, -trackerWaistDistance, 0);
chestNode.attachChild(waistNode);
waistNode.localTransform.setTranslation(0, -waistDistance / 2, 0);
chestNode.attachChild(trackerWaistNode);
trackerWaistNode.localTransform.setTranslation(0, -(trackerWaistDistance - waistDistance / 2), 0);
configMap.put("Head", headShift);
configMap.put("Neck", neckLength);
@@ -107,12 +113,14 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
case "Waist":
waistDistance = newLength;
server.config.setProperty("body.waistDistance", waistDistance);
waistNode.localTransform.setTranslation(0, -waistDistance, 0);
chestNode.localTransform.setTranslation(0, -waistDistance / 2, 0);
waistNode.localTransform.setTranslation(0, -waistDistance / 2, 0);
trackerWaistNode.localTransform.setTranslation(0, -(trackerWaistDistance - waistDistance / 2), 0);
break;
case "Virtual waist":
trackerWaistDistance = newLength;
server.config.setProperty("body.trackerWaistDistance", trackerWaistDistance);
trackerWaistNode.localTransform.setTranslation(0, -trackerWaistDistance, 0);
trackerWaistNode.localTransform.setTranslation(0, -(trackerWaistDistance - waistDistance / 2), 0);
break;
}
}
@@ -137,18 +145,12 @@ public class HumanSkeleonWithWaist extends HumanSkeleton {
hmdNode.localTransform.setRotation(qBuf);
headNode.localTransform.setRotation(qBuf);
wasitTracker.getRotation(qBuf);
chestTracker.getRotation(qBuf);
neckNode.localTransform.setRotation(qBuf);
waistTracker.getRotation(qBuf);
trackerWaistNode.localTransform.setRotation(qBuf);
// Pelvic bone doesn't tilt when humans tilt, unless they really try.
// Can't calculate tilt without additional sensors, so just remove it
// completely.
//qBuf.toAngles(waistAngles);
//waistAngles[0] = 0;
//waistAngles[2] *= 0.2f; // Keep some roll
//qBuf.fromAngles(waistAngles);
chestNode.localTransform.setRotation(qBuf);
waistNode.localTransform.setRotation(qBuf);
}

42
src/main/java/io/eiren/vr/trackers/AdjustedFullTracker.java Normal file
View File

@@ -0,0 +1,42 @@
package io.eiren.vr.trackers;
import com.jme3.math.Quaternion;
public class AdjustedFullTracker extends AdjustedYawTracker {
private final float[] angles = new float[3];
private float yawCorrection = 0;
private float pitchCorrection = 0;
private float rollCorrection = 0;
public AdjustedFullTracker(Tracker tracker) {
super(tracker);
}
@Override
public void adjust(Quaternion reference) {
float[] angles = this.angles;
reference.toAngles(angles);
// Use only yaw HMD rotation
angles[0] = 0;
angles[2] = 0;
Quaternion sensorRotation = new Quaternion();
tracker.getRotation(sensorRotation);
float[] angles2 = new float[3];
sensorRotation.toAngles(angles2);
yawCorrection = angles[1] - angles2[1];
pitchCorrection = angles[0] - angles2[0];
rollCorrection = angles[2] - angles2[2];
}
@Override
protected void adjustInternal(Quaternion store) {
float[] angles = this.angles;
store.toAngles(angles);
angles[0] += pitchCorrection;
angles[1] += yawCorrection;
angles[2] += rollCorrection;
store.fromAngles(angles);
}
}

35
src/main/java/io/eiren/vr/trackers/AdjustedTracker.java
View File

@@ -4,10 +4,9 @@ import com.jme3.math.FastMath;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
public class AdjustedTracker implements Tracker {
public abstract class AdjustedTracker implements Tracker {
public final Tracker tracker;
public final Quaternion adjustment = new Quaternion();
private final Quaternion smoothedQuaternion = new Quaternion();
private float[] angles = new float[3];
protected float[] lastAngles = new float[3];
@@ -18,35 +17,16 @@ public class AdjustedTracker implements Tracker {
public AdjustedTracker(Tracker tracker) {
this.tracker = tracker;
}
@Override
public void loadConfig(TrackerConfig config) {
if(config.adjustment != null)
adjustment.set(config.adjustment);
}
@Override
public void saveConfig(TrackerConfig cfg) {
cfg.adjustment = new Quaternion(adjustment);
public void saveConfig(TrackerConfig config) {
}
public void adjust(Quaternion reference) {
Quaternion targetTrackerRotation = new Quaternion(reference);
// Use only yaw rotation
Vector3f hmdFront = new Vector3f(0, 0, 1);
targetTrackerRotation.multLocal(hmdFront);
hmdFront.multLocal(1, 0, 1).normalizeLocal();
targetTrackerRotation.lookAt(hmdFront, Vector3f.UNIT_Y);
Quaternion sensorRotation = new Quaternion();
tracker.getRotation(sensorRotation);
adjustment.set(sensorRotation).inverseLocal().multLocal(targetTrackerRotation);
confidenceMultiplier = 1.0f / tracker.getConfidenceLevel();
lastAngles[0] = 1000;
}
public abstract void adjust(Quaternion reference);
@Override
public boolean getRotation(Quaternion store) {
@@ -60,10 +40,11 @@ public class AdjustedTracker implements Tracker {
store.set(smoothedQuaternion);
}
}
adjustment.mult(store, store);
adjustInternal(store);
return true;
}
protected abstract void adjustInternal(Quaternion store);
@Override
public boolean getPosition(Vector3f store) {

10
src/main/java/io/eiren/vr/trackers/AdjustedYawTracker.java
View File

@@ -3,6 +3,8 @@ package io.eiren.vr.trackers;
import com.jme3.math.Quaternion;
public class AdjustedYawTracker extends AdjustedTracker {
public final Quaternion adjustment = new Quaternion();
public AdjustedYawTracker(Tracker tracker) {
super(tracker);
@@ -11,7 +13,8 @@ public class AdjustedYawTracker extends AdjustedTracker {
@Override
public void adjust(Quaternion reference) {
Quaternion targetTrackerRotation = new Quaternion(reference);
// Use only yaw HMD rotation
float[] angles = new float[3];
targetTrackerRotation.toAngles(angles);
targetTrackerRotation.fromAngles(0, angles[1], 0);
@@ -27,4 +30,9 @@ public class AdjustedYawTracker extends AdjustedTracker {
confidenceMultiplier = 1.0f / tracker.getConfidenceLevel();
lastAngles[0] = 1000;
}
@Override
protected void adjustInternal(Quaternion store) {
adjustment.mult(store, store);
}
}

18
src/main/java/io/eiren/vr/trackers/TrackersUDPServer.java
View File

@@ -226,6 +226,7 @@ public class TrackersUDPServer extends Thread {
}
System.out.println("[TrackerServer] Sensor " + i + " added with address " + addr);
}
sensor.tracker.setStatus(TrackerStatus.OK);
socket.send(new DatagramPacket(HANDSHAKE_BUFFER, HANDSHAKE_BUFFER.length, handshakePacket.getAddress(), handshakePacket.getPort()));
}
@@ -365,6 +366,15 @@ public class TrackersUDPServer extends Thread {
byte tap = bb.get();
System.out.println("[TrackerServer] Tap packet received from " + tracker.getName() + ": b" + Integer.toBinaryString(tap));
break;
case 14: // PACKET_RESET_REASON
bb.getLong();
byte reason = bb.get();
System.out.println("[TrackerServer] Reset recieved from " + recieve.getSocketAddress() + ": " + reason);
if(sensor == null)
break;
tracker = sensor.tracker;
tracker.setStatus(TrackerStatus.ERROR);
break;
default:
System.out.println("[TrackerServer] Unknown data received: " + packetId + " from " + recieve.getSocketAddress());
break;
@@ -380,9 +390,11 @@ public class TrackersUDPServer extends Thread {
TrackerConnection conn = trackers.get(i);
IMUTracker tracker = conn.tracker;
socket.send(new DatagramPacket(KEEPUP_BUFFER, KEEPUP_BUFFER.length, conn.address));
if(conn.lastPacket + 1000 < System.currentTimeMillis())
tracker.setStatus(TrackerStatus.DISCONNECTED);
else
if(conn.lastPacket + 1000 < System.currentTimeMillis()) {
if(tracker.getStatus() != TrackerStatus.DISCONNECTED) {
tracker.setStatus(TrackerStatus.DISCONNECTED);
}
} else if(tracker.getStatus() != TrackerStatus.ERROR && tracker.getStatus() != TrackerStatus.BUSY)
tracker.setStatus(TrackerStatus.OK);
if(tracker.serialBuffer.length() > 0) {
if(tracker.lastSerialUpdate + 500L < System.currentTimeMillis()) {