Publish tracking data through VMC Protocol

alvr/server_core/src/tracking/mod.rs

@@ -1,8 +1,10 @@
 mod body;
 mod face;
+mod vmc;
 
 pub use body::*;
 pub use face::*;
+pub use vmc::*;
 
 use crate::{
     connection::STREAMING_RECV_TIMEOUT,
@@ -21,7 +23,7 @@ use alvr_events::{EventType, TrackingEvent};
 use alvr_packets::{FaceData, Tracking};
 use alvr_session::{
     settings_schema::Switch, BodyTrackingConfig, HeadsetConfig, PositionRecenteringMode,
-    RotationRecenteringMode, Settings,
+    RotationRecenteringMode, Settings, VMCConfig,
 };
 use alvr_sockets::StreamReceiver;
 use std::{
@@ -325,6 +327,12 @@ pub fn tracking_loop(
             BodyTrackingSink::new(config.sink, initial_settings.connection.osc_local_port).ok()
         });
 
+    let mut vmc_sink = initial_settings
+        .headset
+        .vmc
+        .into_option()
+        .and_then(|config| VMCSink::new(config).ok());
+
     while is_streaming() {
         let data = match tracking_receiver.recv(STREAMING_RECV_TIMEOUT) {
             Ok(tracking) => tracking,
@@ -471,6 +479,43 @@ pub fn tracking_loop(
             })
             .ok();
 
+        let publish_vmc = matches!(
+            SESSION_MANAGER.read().settings().headset.vmc,
+            Switch::Enabled(VMCConfig { publish: true, .. })
+        );
+        if publish_vmc {
+            let orientation_correction = matches!(
+                SESSION_MANAGER.read().settings().headset.vmc,
+                Switch::Enabled(VMCConfig {
+                    orientation_correction: true,
+                    ..
+                })
+            );
+
+            if let Some(sink) = &mut vmc_sink {
+                let tracking_manager_lock = ctx.tracking_manager.read();
+                let device_motions = device_motion_keys
+                    .iter()
+                    .filter_map(move |id| {
+                        Some((
+                            *id,
+                            tracking_manager_lock
+                                .get_device_motion(*id, timestamp)
+                                .unwrap(),
+                        ))
+                    })
+                    .collect::<Vec<(u64, DeviceMotion)>>();
+
+                if let Some(skeleton) = tracking.hand_skeletons[0] {
+                    sink.send_hand_tracking(HandType::Left, skeleton, orientation_correction);
+                }
+                if let Some(skeleton) = tracking.hand_skeletons[1] {
+                    sink.send_hand_tracking(HandType::Right, skeleton, orientation_correction);
+                }
+                sink.send_tracking(&device_motions, orientation_correction);
+            }
+        }
+
         let track_body = matches!(
             SESSION_MANAGER.read().settings().headset.body_tracking,
             Switch::Enabled(BodyTrackingConfig { tracked: true, .. })

alvr/server_core/src/tracking/vmc.rs

@@ -0,0 +1,181 @@
+use alvr_common::{
+    anyhow::Result, glam::Quat, once_cell::sync::Lazy, DeviceMotion, Pose, BODY_CHEST_ID,
+    BODY_HIPS_ID, BODY_LEFT_ELBOW_ID, BODY_LEFT_FOOT_ID, BODY_LEFT_KNEE_ID, BODY_RIGHT_ELBOW_ID,
+    BODY_RIGHT_FOOT_ID, BODY_RIGHT_KNEE_ID, HAND_LEFT_ID, HAND_RIGHT_ID, HEAD_ID,
+};
+use rosc::{OscMessage, OscPacket, OscType};
+use std::{collections::HashMap, net::UdpSocket};
+
+use alvr_session::VMCConfig;
+
+pub use crate::tracking::HandType;
+
+// Transform DeviceMotion into Unity HumanBodyBones
+// https://docs.unity3d.com/ScriptReference/HumanBodyBones.html
+static DEVICE_MOTIONS_VMC_MAP: Lazy<HashMap<u64, &'static str>> = Lazy::new(|| {
+    HashMap::from([
+        (*HAND_LEFT_ID, "LeftHand"),
+        (*HAND_RIGHT_ID, "RightHand"),
+        (*BODY_CHEST_ID, "Chest"),
+        (*BODY_HIPS_ID, "Hips"),
+        (*BODY_LEFT_ELBOW_ID, "LeftLowerArm"),
+        (*BODY_RIGHT_ELBOW_ID, "RightLowerArm"),
+        (*BODY_LEFT_KNEE_ID, "LeftLowerLeg"),
+        (*BODY_LEFT_FOOT_ID, "LeftFoot"),
+        (*BODY_RIGHT_KNEE_ID, "RightLowerLeg"),
+        (*BODY_RIGHT_FOOT_ID, "RightFoot"),
+        (*HEAD_ID, "Head"),
+    ])
+});
+
+static DEVICE_MOTIONS_ROTATION_MAP: Lazy<HashMap<u64, Quat>> = Lazy::new(|| {
+    HashMap::from([
+        (
+            *HAND_LEFT_ID,
+            Quat::from_xyzw(-0.03538, 0.25483, -0.00000, -0.96634),
+        ),
+        (
+            *HAND_RIGHT_ID,
+            Quat::from_xyzw(-0.05859, -0.20524, -0.00000, 0.97696),
+        ),
+        (
+            *BODY_CHEST_ID,
+            Quat::from_xyzw(-0.49627, 0.49516, -0.43469, -0.56531),
+        ),
+        (
+            *BODY_HIPS_ID,
+            Quat::from_xyzw(-0.49274, 0.49568, -0.42416, -0.57584),
+        ),
+        (
+            *BODY_RIGHT_ELBOW_ID,
+            Quat::from_xyzw(-0.63465, -0.11567, 0.00000, 0.76410),
+        ),
+        (
+            *BODY_LEFT_KNEE_ID,
+            Quat::from_xyzw(0.51049, 0.47862, 0.42815, -0.57185),
+        ),
+        (
+            *BODY_LEFT_FOOT_ID,
+            Quat::from_xyzw(-0.59103, 0.38818, 0.00000, -0.7071100),
+        ),
+        (
+            *BODY_RIGHT_KNEE_ID,
+            Quat::from_xyzw(-0.52823, 0.45434, -0.58530, -0.41470),
+        ),
+        (
+            *BODY_RIGHT_FOOT_ID,
+            Quat::from_xyzw(0.70228, -0.08246, 0.7071100, 0.00000),
+        ),
+    ])
+});
+
+static HAND_SKELETON_VMC_MAP: Lazy<[[(usize, &'static str); 1]; 2]> =
+    Lazy::new(|| [[(0, "LeftHand")], [(0, "RightHand")]]);
+
+static HAND_SKELETON_ROTATIONS: Lazy<[HashMap<usize, Quat>; 2]> = Lazy::new(|| {
+    [
+        HashMap::from([(0, Quat::from_xyzw(-0.03566, 0.25481, 0.00000, -0.96633))]),
+        HashMap::from([(0, Quat::from_xyzw(-0.05880, -0.20574, -0.00000, 0.97684))]),
+    ]
+});
+
+pub struct VMCSink {
+    socket: Option<UdpSocket>,
+}
+
+impl VMCSink {
+    pub fn new(config: VMCConfig) -> Result<Self> {
+        let socket = UdpSocket::bind("0.0.0.0:0")?;
+        socket.connect(format!("{}:{}", config.host, config.port))?;
+
+        Ok(Self {
+            socket: Some(socket),
+        })
+    }
+
+    fn send_osc_message(&self, path: &str, args: Vec<OscType>) {
+        if let Some(socket) = &self.socket {
+            socket
+                .send(
+                    &rosc::encoder::encode(&OscPacket::Message(OscMessage {
+                        addr: path.into(),
+                        args,
+                    }))
+                    .unwrap(),
+                )
+                .ok();
+        }
+    }
+
+    pub fn send_hand_tracking(
+        &mut self,
+        hand_type: HandType,
+        skeleton: [Pose; 26],
+        orientation_correction: bool,
+    ) {
+        let hand_id = hand_type as usize;
+        for (part, vmc_str) in HAND_SKELETON_VMC_MAP[hand_id] {
+            let corrected_orientation = {
+                let mut q = skeleton[part].orientation;
+                if orientation_correction {
+                    if HAND_SKELETON_ROTATIONS[hand_id].contains_key(&part) {
+                        q *= *HAND_SKELETON_ROTATIONS[hand_id].get(&part).unwrap();
+                    }
+                    q.z = -q.z;
+                    q.w = -q.w;
+                }
+                q
+            };
+
+            self.send_osc_message(
+                "/VMC/Ext/Bone/Pos",
+                vec![
+                    OscType::String(vmc_str.to_string()),
+                    OscType::Float(skeleton[part].position.x),
+                    OscType::Float(skeleton[part].position.y),
+                    OscType::Float(skeleton[part].position.z),
+                    OscType::Float(corrected_orientation.x),
+                    OscType::Float(corrected_orientation.y),
+                    OscType::Float(corrected_orientation.z),
+                    OscType::Float(corrected_orientation.w),
+                ],
+            );
+        }
+    }
+
+    pub fn send_tracking(
+        &mut self,
+        device_motions: &[(u64, DeviceMotion)],
+        orientation_correction: bool,
+    ) {
+        for (id, motion) in device_motions {
+            if DEVICE_MOTIONS_VMC_MAP.contains_key(id) {
+                let corrected_orientation = {
+                    let mut q = motion.pose.orientation;
+                    if orientation_correction {
+                        if DEVICE_MOTIONS_ROTATION_MAP.contains_key(id) {
+                            q *= *DEVICE_MOTIONS_ROTATION_MAP.get(id).unwrap();
+                        }
+                        q.z = -q.z;
+                        q.w = -q.w;
+                    }
+                    q
+                };
+
+                self.send_osc_message(
+                    "/VMC/Ext/Bone/Pos",
+                    vec![
+                        OscType::String(DEVICE_MOTIONS_VMC_MAP.get(id).unwrap().to_string()),
+                        OscType::Float(motion.pose.position.x),
+                        OscType::Float(motion.pose.position.y),
+                        OscType::Float(motion.pose.position.z),
+                        OscType::Float(corrected_orientation.x),
+                        OscType::Float(corrected_orientation.y),
+                        OscType::Float(corrected_orientation.z),
+                        OscType::Float(corrected_orientation.w),
+                    ],
+                );
+            }
+        }
+    }
+}

alvr/session/src/settings.rs

@@ -740,6 +740,18 @@ pub struct BodyTrackingConfig {
     pub tracked: bool,
 }
 
+#[derive(SettingsSchema, Serialize, Deserialize, Clone)]
+#[schema(collapsible)]
+pub struct VMCConfig {
+    pub host: String,
+    pub port: u16,
+    #[schema(strings(help = "Turn this off to temporarily pause sending data."))]
+    #[schema(flag = "real-time")]
+    pub publish: bool,
+    #[schema(flag = "real-time")]
+    pub orientation_correction: bool,
+}
+
 #[derive(SettingsSchema, Serialize, Deserialize, Clone, PartialEq, Eq)]
 pub enum ControllersEmulationMode {
     #[schema(strings(display_name = "Rift S Touch"))]
@@ -1037,6 +1049,10 @@ Tilted: the world gets tilted when long pressing the oculus button. This is usef
 
     #[schema(flag = "steamvr-restart")]
     pub body_tracking: Switch<BodyTrackingConfig>,
+
+    #[schema(flag = "steamvr-restart")]
+    #[schema(strings(display_name = "VMC"))]
+    pub vmc: Switch<VMCConfig>,
 }
 
 #[derive(SettingsSchema, Serialize, Deserialize, Clone, Copy)]
@@ -1565,6 +1581,16 @@ pub fn session_settings_default() -> SettingsDefault {
                     tracked: true,
                 },
             },
+            vmc: SwitchDefault {
+                enabled: false,
+                content: VMCConfigDefault {
+                    gui_collapsed: true,
+                    host: "127.0.0.1".into(),
+                    port: 39539,
+                    publish: true,
+                    orientation_correction: true,
+                },
+            },
             controllers: SwitchDefault {
                 enabled: true,
                 content: ControllersConfigDefault {