Here's an unsupported script that could help get you started on a script only solution for importing our OptiTrack data stream into Unity. This is a known working script against against 1.7.x, and should work with 1.8.x. Please note however that when doing direct depacketization, if the NatNet bitstream syntax changes (e.g. we added something) you'll need to update your script.
Also this script is provided "as-is" - we dont support it at this time.
1. Queries Arena for the model description (e.g. skeletons, markersets, etc)
2. Creates a Skeleton object from the data description
3. updates the skeleton joint pos/ori in a separate thread from socket events
Code: Select all
using System;
using System.Net;
using System.Net.Sockets;
using UnityEngine;
public class MyStateObject
{
public Socket workSocket = null;
public const int BUFFER_SIZE = 65507;
public byte[] buffer = new byte[BUFFER_SIZE];
}
// OptiTrackUDPClient is a class for connecting to OptiTrack Arena Skeleton data
// and storing in a general Skeleton class object for access by Unity characters
public class OptiTrackUDPClient
{
public int dataPort = 1511;
public int commandPort = 1510;
public string multicastIPAddress = "239.255.42.99";
public string localIPAddress = "127.0.0.1";
public bool bNewData = false;
public Skeleton skelTarget = null;
Socket sockData = null;
Socket sockCommand = null;
String strFrame = "";
public OptiTrackUDPClient ()
{
}
public bool Connect()
{
IPEndPoint ipep;
MyStateObject so;
Debug.Log("[UDPClient] Connecting.");
// create data socket
sockData = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
sockData.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
ipep = new IPEndPoint(IPAddress.Parse(localIPAddress), dataPort);
try
{
sockData.Bind(ipep);
}
catch (Exception ex)
{
Debug.Log("bind exception : " + ex.Message);
}
// connect socket to multicast group
IPAddress ip = IPAddress.Parse(multicastIPAddress);
sockData.SetSocketOption(SocketOptionLevel.IP, SocketOptionName.AddMembership, new MulticastOption(ip, IPAddress.Parse(localIPAddress)));
so = new MyStateObject();
#if true
// asynch - begin listening
so.workSocket = sockData;
sockData.BeginReceive(so.buffer, 0, MyStateObject.BUFFER_SIZE, 0, new AsyncCallback(AsyncReceiveCallback), so);
#else
// synch - read 1 frame
int nBytesRead = s.Receive(so.buffer);
strFrame = String.Format("Received Bytes : {0}\n", nBytesRead);
if(nBytesRead > 0)
ReadPacket(so.buffer);
textBox1.Text = strFrame;
#endif
// create command socket
sockCommand = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
ipep = new IPEndPoint(IPAddress.Parse(localIPAddress), 0);
try
{
sockCommand.Bind(ipep);
}
catch (Exception ex)
{
Debug.Log("bind exception : " + ex.Message);
}
// asynch - begin listening
so = new MyStateObject();
so.workSocket = sockCommand;
sockCommand.BeginReceive(so.buffer, 0, MyStateObject.BUFFER_SIZE, 0, new AsyncCallback(AsyncReceiveCallback), so);
return true;
}
public bool RequestDataDescriptions()
{
if(sockCommand != null)
{
Byte[] message = new Byte[100];
int offset = 0;
ushort[] val = new ushort[1];
val[0] = 4;
Buffer.BlockCopy(val, 0, message, offset, 1*sizeof(ushort));
offset += sizeof(ushort);
val[0] = 0;
Buffer.BlockCopy(val, 0, message, offset, 1*sizeof(ushort));
offset += sizeof(ushort);
IPEndPoint ipep = new IPEndPoint(IPAddress.Parse(localIPAddress), commandPort);
int iBytesSent = sockCommand.SendTo(message, ipep);
Debug.Log("[UDPClient] sent RequestDescription . (Bytes sent:" + iBytesSent + ")");
}
return true;
}
public bool RequestFrameOfData()
{
if(sockCommand != null)
{
Byte[] message = new Byte[100];
int offset = 0;
ushort[] val = new ushort[1];
val[0] = 6;
Buffer.BlockCopy(val, 0, message, offset, 1*sizeof(ushort));
offset += sizeof(ushort);
val[0] = 0;
Buffer.BlockCopy(val, 0, message, offset, 1*sizeof(ushort));
offset += sizeof(ushort);
IPEndPoint ipep = new IPEndPoint(IPAddress.Parse(localIPAddress), commandPort);
int iBytesSent = sockCommand.SendTo(message, ipep);
Debug.Log("[UDPClient] Sent RequestFrameOfData. (Bytes sent:" + iBytesSent + ")");
}
return true;
}
// Async socket reader callback - called by .net when socket async receive procedure receives a message
private void AsyncReceiveCallback(IAsyncResult ar)
{
MyStateObject so = (MyStateObject)ar.AsyncState;
Socket s = so.workSocket;
int read = s.EndReceive(ar);
//Debug.Log("[UDPClient] Received Packet (" + read + " bytes)");
if (read > 0)
{
// unpack the data
ReadPacket(so.buffer);
if(s == sockData)
bNewData = true; // indicate to update character
// listen for next frame
s.BeginReceive(so.buffer, 0, MyStateObject.BUFFER_SIZE, 0, new AsyncCallback(AsyncReceiveCallback), so);
}
}
private void ReadPacket(Byte[] b)
{
int offset = 0;
int nBytes = 0;
int[] iData = new int[100];
float[] fData = new float[500];
char[] cData = new char[500];
Buffer.BlockCopy(b, offset, iData, 0, 2); offset += 2;
int messageID = iData[0];
Buffer.BlockCopy(b, offset, iData, 0, 2); offset += 2;
nBytes = iData[0];
//Debug.Log("[UDPClient] Processing Received Packet (Message ID : " + messageID + ")");
if(messageID == 5) // Data descriptions
{
strFrame = ("[UDPClient] Read DataDescriptions");
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
strFrame += String.Format("Dataset Count: {0}\n", iData[0]);
int nDatasets = iData[0];
for(int i=0; i < nDatasets; i++)
{
//print("Dataset %d\n", i);
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
strFrame += String.Format("Dataset # {0} (type: {1})\n", i, iData[0]);
int type = iData[0];
if(type == 0) // markerset
{
// name
string strName = "";
while(b[offset] != '\0')
{
Buffer.BlockCopy(b, offset, cData, 0, 1); offset += 1;
strName += cData[0];
}
offset += 1;
strFrame += String.Format("MARKERSET (Name: {0})\n", strName);
// marker data
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
strFrame += String.Format("marker count: {0}\n", iData[0]);
int nMarkers = iData[0];
for(int j=0; j < nMarkers; j++)
{
strName = "";
while(b[offset] != '\0')
{
Buffer.BlockCopy(b, offset, cData, 0, 1); offset += 1;
strName += cData[0];
}
offset +=1;
strFrame += String.Format("Name : {0}\n", strName);
}
}
else if(type ==1) // rigid body
{
/*
if(major >= 2)
{
// name
char szName[MAX_NAMELENGTH];
strcpy(szName, ptr);
ptr += strlen(ptr) + 1;
printf("Name: %s\n", szName);
}
int ID = 0; memcpy(&ID, ptr, 4); ptr +=4;
printf("ID : %d\n", ID);
int parentID = 0; memcpy(&parentID, ptr, 4); ptr +=4;
printf("Parent ID : %d\n", parentID);
float xoffset = 0; memcpy(&xoffset, ptr, 4); ptr +=4;
printf("X Offset : %3.2f\n", xoffset);
float yoffset = 0; memcpy(&yoffset, ptr, 4); ptr +=4;
printf("Y Offset : %3.2f\n", yoffset);
float zoffset = 0; memcpy(&zoffset, ptr, 4); ptr +=4;
printf("Z Offset : %3.2f\n", zoffset);
*/
}
else if(type ==2) // skeleton
{
InitializeSkeleton(b, offset);
}
} // next dataset
Debug.Log(strFrame);
}
else if (messageID == 7) // Frame of Mocap Data
{
strFrame = "[UDPClient] Read FrameOfMocapData\n";
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
strFrame += String.Format("Frame # : {0}\n", iData[0]);
// MarkerSets
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
int nMarkerSets = iData[0];
strFrame += String.Format("MarkerSets # : {0}\n", iData[0]);
for (int i = 0; i < nMarkerSets; i++)
{
String strName = "";
int nChars = 0;
while (b[offset + nChars] != '\0')
{
nChars++;
}
strName = System.Text.Encoding.ASCII.GetString(b, offset, nChars);
offset += nChars + 1;
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
strFrame += String.Format("Marker Count : {0}\n", iData[0]);
nBytes = iData[0] * 3 * 4;
Buffer.BlockCopy(b, offset, fData, 0, nBytes); offset += nBytes;
}
// Other Markers
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
int nOtherMarkers = iData[0];
strFrame += String.Format("Other Markers : {0}\n", iData[0]);
nBytes = iData[0] * 3 * 4;
Buffer.BlockCopy(b, offset, fData, 0, nBytes); offset += nBytes;
// Rigid Bodies
RigidBody rb = new RigidBody();
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
int nRigidBodies = iData[0];
strFrame += String.Format("Rigid Bodies : {0}\n", iData[0]);
for (int i = 0; i < nRigidBodies; i++)
{
ReadRB(b, ref offset, rb);
}
// Skeletons
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
int nSkeletons = iData[0];
strFrame += String.Format("Skeletons : {0}\n", iData[0]);
for (int i = 0; i < nSkeletons; i++)
{
// ID
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
skelTarget.ID = iData[0];
// # rbs (bones) in skeleton
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
skelTarget.nBones = iData[0];
for (int j = 0; j < skelTarget.nBones; j++)
{
ReadRB(b, ref offset, skelTarget.bones[j]);
}
}
// frame latency
Buffer.BlockCopy(b, offset, fData, 0, 4); offset += 4;
// end of data (EOD) tag
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
//Debug.Log(strFrame);
// debug
String str = String.Format("Skel ID : {0}", skelTarget.ID);
for (int i = 0; i < skelTarget.nBones; i++)
{
String st = String.Format(" Bone {0}: ID: {1} raw pos ({2:F2},{3:F2},{4:F2}) raw ori ({5:F2},{6:F2},{7:F2},{8:F2})",
i, skelTarget.bones[i].ID,
skelTarget.bones[i].pos[0], skelTarget.bones[i].pos[1], skelTarget.bones[i].pos[2],
skelTarget.bones[i].ori[0], skelTarget.bones[i].ori[1], skelTarget.bones[i].ori[2], skelTarget.bones[i].ori[3]);
str += "\n" + st;
}
//Debug.Log(str);
if(skelTarget.bNeedBoneLengths)
skelTarget.UpdateBoneLengths();
}
else if(messageID == 100)
{
Debug.Log("Packet Read: Unrecognized Request.");
}
}
// Unpack RigidBody data
private void ReadRB(Byte[] b, ref int offset, RigidBody rb)
{
int[] iData = new int[100];
float[] fData = new float[100];
// RB ID
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
int iSkelID = iData[0] >> 16; // hi 16 bits = ID of bone's parent skeleton
int iBoneID = iData[0] & 0xffff; // lo 16 bits = ID of bone
//rb.ID = iData[0]; // already have it from data descriptions
// RB pos
float[] pos = new float[3];
Buffer.BlockCopy(b, offset, pos, 0, 4 * 3); offset += 4 * 3;
rb.pos.x = pos[0]; rb.pos[1] = pos[1]; rb.pos[2] = pos[2];
// RB ori
float[] ori = new float[4];
Buffer.BlockCopy(b, offset, ori, 0, 4 * 4); offset += 4 * 4;
rb.ori.x = ori[0]; rb.ori.y = ori[1]; rb.ori.z = ori[2]; rb.ori.w = ori[3];
// RB's markers
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
int nMarkers = iData[0];
Buffer.BlockCopy(b, offset, fData, 0, 4 * 3 * nMarkers); offset += 4 * 3 * nMarkers;
// RB's marker ids
Buffer.BlockCopy(b, offset, iData, 0, 4 * nMarkers); offset += 4 * nMarkers;
// RB's marker sizes
Buffer.BlockCopy(b, offset, fData, 0, 4 * nMarkers); offset += 4 * nMarkers;
// RB mean error
Buffer.BlockCopy(b, offset, fData, 0, 4); offset += 4;
}
void InitializeSkeleton(Byte[] b, int offset)
{
int[] iData = new int[100];
float[] fData = new float[500];
char[] cData = new char[500];
string strName = "";
while(b[offset] != '\0')
{
Buffer.BlockCopy(b, offset, cData, 0, 1); offset += 1;
strName += cData[0];
}
offset += 1;
strFrame += String.Format("SKELETON (Name: {0})\n", strName);
skelTarget.name = strName;
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
strFrame += String.Format("SkeletonID: {0}\n", iData[0]);
skelTarget.ID = iData[0];
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
strFrame += String.Format("nRigidBodies: {0}\n", iData[0]);
skelTarget.nBones = iData[0];
for(int j=0; j< skelTarget.nBones; j++)
{
// RB name
string strRBName = "";
while(b[offset] != '\0')
{
Buffer.BlockCopy(b, offset, cData, 0, 1); offset += 1;
strRBName += cData[0];
}
offset += 1;
strFrame += String.Format("RBName: {0}\n", strRBName);
skelTarget.bones[j].name = strRBName;
// RB ID
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
int iSkelID = iData[0] >> 16; // hi 16 bits = ID of bone's parent skeleton
int iBoneID = iData[0] & 0xffff; // lo 16 bits = ID of bone
//Debug.Log("RBID:" + iBoneID + " SKELID:"+iSkelID);
strFrame += String.Format("RBID: {0}\n", iBoneID);
skelTarget.bones[j].ID = iBoneID;
// RB Parent
Buffer.BlockCopy(b, offset, iData, 0, 4); offset += 4;
strFrame += String.Format("RB Parent ID: {0}\n", iData[0]);
skelTarget.bones[j].parentID = iData[0];
// RB local position offset
Vector3 localPos;
Buffer.BlockCopy(b, offset, fData, 0, 4); offset += 4;
strFrame += String.Format("X Offset: {0}\n", fData[0]);
localPos.x = fData[0];
Buffer.BlockCopy(b, offset, fData, 0, 4); offset += 4;
strFrame += String.Format("Y Offset: {0}\n", fData[0]);
localPos.y = fData[0];
Buffer.BlockCopy(b, offset, fData, 0, 4); offset += 4;
strFrame += String.Format("Z Offset: {0}\n", fData[0]);
localPos.z = fData[0];
skelTarget.bones[j].pos = localPos;
//Debug.Log("[UDPClient] Added Bone: " + skelTarget.bones[j].name);
}
skelTarget.bHasHierarchyDescription = true;
}
}
Code: Select all
using System;
using System.Collections;
using System.Collections.Generic;
using System.Net;
using UnityEngine;
// bone
public class RigidBody
{
public string name = "";
public int ID = -1;
public int parentID = -1;
public float length = 1.0f;
public Vector3 pos;
public Quaternion ori;
public Quaternion initOri;
public Transform trans;
}
// skeleton
public class Skeleton
{
public string name = "";
public int ID = -1;
public int nBones = 0;
public RigidBody[] bones = new RigidBody[200];
public bool bHasHierarchyDescription = false;
public bool bNeedBoneLengths = true;
public float fUnitConversion = .001f; // arena mm to m
public float refLength;
public Dictionary<int,GameObject> RBToGameObject = new Dictionary<int, GameObject>();
public Dictionary<GameObject,int> GameObjectToRB = new Dictionary<GameObject,int>();
public GameObject rootObject = null;
public Skeleton()
{
for(int i=0; i < 200; i++)
{
bones[i] = new RigidBody();
}
}
public virtual void UpdateBonePosOriFromFile(int frameIndex)
{
}
// Create a skeleton from an existing unity rig
public void CreateFromTransform(Transform t)
{
name = t.name;
AddBones(t, -1);
bNeedBoneLengths = false;
bHasHierarchyDescription = true;
Debug.Log("[Skeleton] Created skeleton from existing transforms (name:" + name + " Bones:"+nBones+")");
}
private void AddBones(Transform t, int parentIndex)
{
int index = nBones;
bones[index].name = t.name;
bones[index].ID = index;
bones[index].parentID = parentIndex;
bones[index].pos = t.localPosition;
bones[index].ori = t.localRotation;
bones[index].initOri = t.localRotation; // initial ori required to get character skeleton into init-pose (aka t-pose, neutral pose)
bones[index].trans = t;
if(t.childCount == 1)
{
bones[index].length = t.GetChild(0).localPosition.magnitude;
// use upper leg as a "reference length" for this skeleton
if(bones[index].name.Contains("Thigh") || bones[index].name.Contains("UpLeg"))
{
refLength = bones[index].length;
Debug.Log(refLength);
}
}
nBones++;
//Debug.Log("[Skeleton] Created Bone: " + bones[index].name + "(length: "+ bones[index].length+")");
RBToGameObject[bones[index].ID] = t.gameObject;
GameObjectToRB[t.gameObject] = bones[index].ID;
foreach (Transform tChild in t)
{
AddBones(tChild, index);
}
}
public RigidBody GetBone(string boneName)
{
for(int i=0; i < nBones; i++)
{
if(bones[i].name == boneName)
return bones[i];
}
return null;
}
public RigidBody GetBone(int id)
{
for(int i=0; i < nBones; i++)
{
if(bones[i].ID == id)
return bones[i];
}
return null;
}
public void UpdateBoneLengths()
{
if(bHasHierarchyDescription)
{
for(int i=0; i< nBones; i++)
{
RigidBody rb = bones[i];
if(rb.parentID >= 0)
{
RigidBody rbParent = GetBone(rb.parentID);
if(rbParent != null)
{
// todo : where an rb has multiple children, traditional bone length is undefined
// for purpose of retargeting, could define it as average of all parent-child lengths, or min or max.
Vector3 v = rb.pos; // pos is already a local xform - no need to subtract out
float fLength = v.magnitude;
rbParent.length = fLength * fUnitConversion;
//Debug.Log("[Skeleton] Updated Bone Length (Bone:" + rbParent.name + " ID:" + rbParent.ID + " Length:"+rbParent.length+")");
if(rbParent.name.Contains("high"))
{
refLength = rbParent.length;
Debug.Log(refLength);
}
}
}
}
bNeedBoneLengths = false;
Debug.Log("[Skeleton] Updated Bone Length (Skeleton:" + name + ")");
}
}
}
// custom skeleton class - used to define bone mapping between a mocap skeleton source (e.g. Arena)
// and a character rig (e.g. standard Autodesk HIK rig)
public class ArenaSkeleton : Skeleton
{
// IDs from Arena data stream
public enum ArenaBoneID {
hip = 1,
abd,
chest,
neck,
head,
headend,
lshoulder,
luarm,
lfarm,
lhand,
lhandend,
rshoulder,
ruarm,
rfarm,
rhand,
rhandend,
lthigh,
lshin,
lfoot,
lfootend,
rthigh,
rshin,
rfoot,
rfootend
}
public static Dictionary<int,string> ArenaToFBX = new Dictionary<int, string>(); // Arena to custom FBX
public static Dictionary<int,string> ArenaToHIK = new Dictionary<int, string>(); // Arena to standard HIK model
static ArenaSkeleton()
{
// Arena skeleton to standard Maya/Unity HIK model
ArenaToHIK.Add((int)ArenaBoneID.hip, "Hips");
ArenaToHIK.Add((int)ArenaBoneID.abd, "Spine");
ArenaToHIK.Add((int)ArenaBoneID.chest, "Spine2");
ArenaToHIK.Add((int)ArenaBoneID.neck, "Neck");
ArenaToHIK.Add((int)ArenaBoneID.head, "Head");
ArenaToHIK.Add((int)ArenaBoneID.lshoulder, "LeftShoulder");
ArenaToHIK.Add((int)ArenaBoneID.luarm, "LeftArm");
ArenaToHIK.Add((int)ArenaBoneID.lfarm, "LeftForeArm");
ArenaToHIK.Add((int)ArenaBoneID.lhand, "LeftHand");
ArenaToHIK.Add((int)ArenaBoneID.rshoulder, "RightShoulder");
ArenaToHIK.Add((int)ArenaBoneID.ruarm, "RightArm");
ArenaToHIK.Add((int)ArenaBoneID.rfarm, "RightForeArm");
ArenaToHIK.Add((int)ArenaBoneID.rhand, "RightHand");
ArenaToHIK.Add((int)ArenaBoneID.lthigh, "LeftUpLeg");
ArenaToHIK.Add((int)ArenaBoneID.lshin, "LeftLeg");
ArenaToHIK.Add((int)ArenaBoneID.lfoot, "LeftFoot");
ArenaToHIK.Add((int)ArenaBoneID.lfootend, "LeftToeBase");
ArenaToHIK.Add((int)ArenaBoneID.rthigh, "RightUpLeg");
ArenaToHIK.Add((int)ArenaBoneID.rshin, "RightLeg");
ArenaToHIK.Add((int)ArenaBoneID.rfoot, "RightFoot");
ArenaToHIK.Add((int)ArenaBoneID.rfootend, "RightToeBase");
// Arena to custom FBX model
// TODO: modify to support your custom FBX character here
ArenaToFBX.Add((int)ArenaBoneID.hip, "Hips");
ArenaToFBX.Add((int)ArenaBoneID.abd, "Torso_Lower");
ArenaToFBX.Add((int)ArenaBoneID.chest, "Torso_Middle");
ArenaToFBX.Add((int)ArenaBoneID.neck, "Torso_Uppper");
ArenaToFBX.Add((int)ArenaBoneID.head, "Head");
ArenaToFBX.Add((int)ArenaBoneID.headend, "HeadEnd");
ArenaToFBX.Add((int)ArenaBoneID.lshoulder, "LeftShoulder");
ArenaToFBX.Add((int)ArenaBoneID.luarm, "Bicep_L");
ArenaToFBX.Add((int)ArenaBoneID.lfarm, "Forearm_L");
ArenaToFBX.Add((int)ArenaBoneID.lhand, "Wrist_L");
ArenaToFBX.Add((int)ArenaBoneID.rshoulder, "RightShoulder");
ArenaToFBX.Add((int)ArenaBoneID.ruarm, "Bicep_R");
ArenaToFBX.Add((int)ArenaBoneID.rfarm, "Forearm_R");
ArenaToFBX.Add((int)ArenaBoneID.rhand, "Wrist_R");
ArenaToFBX.Add((int)ArenaBoneID.lthigh, "Thigh_L");
ArenaToFBX.Add((int)ArenaBoneID.lshin, "Shin_L");
ArenaToFBX.Add((int)ArenaBoneID.lfoot, "Foot_L");
ArenaToFBX.Add((int)ArenaBoneID.lfootend, "Toes_L");
ArenaToFBX.Add((int)ArenaBoneID.rthigh, "Thigh_R");
ArenaToFBX.Add((int)ArenaBoneID.rshin, "Shin_R");
ArenaToFBX.Add((int)ArenaBoneID.rfoot, "Foot_R");
ArenaToFBX.Add((int)ArenaBoneID.rfootend, "Toes_R");
}
}