public void TestSkeletonGeneration()
{
MAvatarDescription desc = IntermediateSkeleton.GenerateFromDescriptionFile("TestAvatar");
Assert.AreEqual(desc.AvatarID, "TestAvatar");
Assert.AreEqual(desc.ZeroPosture.Joints.Count, 19);
IntermediateSkeleton skeleton = new IntermediateSkeleton();
skeleton.InitializeAnthropometry(desc);
Assert.AreEqual(desc, skeleton.GetAvatarDescription(desc.AvatarID));
string[] jointnames = new string[] { "S1L5Joint", "T12L12Joint", "T1T2Joint", "C4C5Joint", "HeadJoint", "LeftShoulder", "LeftElbow", "LeftWrist", "RightShoulder", "RightElbow", "RightWrist", "LeftHip", "LeftKnee", "LeftAnkle", "LeftBall", "RightHip", "RightKnee", "RightAnkle", "RightBall" };
for (int i = 0; i < desc.ZeroPosture.Joints.Count; i++)
{
MJoint joint = desc.ZeroPosture.Joints[i];
Assert.AreEqual(joint.ID.ToString(), jointnames[i]);
}
}
public void TestRootPosition()
{
MAvatarDescription desc = IntermediateSkeleton.GenerateFromDescriptionFile("TestAvatar");
IntermediateSkeleton skeleton = new IntermediateSkeleton();
skeleton.InitializeAnthropometry(desc);
List <double> rotationValues = new List <double>()
{
0, 0, 0, 0.86041, -0.01303, 0.50383, 0.07533, //S1L5
0.00000, 0.00000, 0.00000, 1.00000, 0.00000, 0.00000, 0.00000, // T12L12
0.00000, 0.00000, 0.00000, 1.00000, 0.00000, 0.00000, 0.00000, // T1T2
0.00000, 0.00000, 0.00000, 1.00000, 0.00000, 0.00000, 0.00000, //C4C5
0.00000, 0.00000, 0.00000, 0.98890, 0.04908, -0.13945, -0.01508, // Head
0.00000, 0.00000, 0.00000, 0.74914, -0.35251, 0.02895, 0.56007, // LeftShoulder
0.98560, 0.11136, -0.00962, 0.12689, // Left ELbow
0.96542, -0.01250, 0.25953, 0.02139, // Left Wrist
0.00000, 0.00000, 0.00000, 0.74411, 0.10420, 0.26279, -0.60530,
0.95158, 0.28073, 0.07735, -0.09850, // Right Elbow
0.99256, -0.00379, 0.11897, -0.02548, // right wrist
0.94999, -0.28306, 0.12805, 0.03154, // Left hip
0.97503, 0.22205, 0.00000, -0.00001, // Knee
0.99439, -0.07404, 0.06580, 0.03709, // Ankle
1.00000, 0.00000, 0.00000, 0.00000, // Toes
0.99694, 0.07053, -0.02371, 0.02406, // Right Hip
0.91716, 0.39852, 0.00000, 0.00000, // Knee
0.99076, -0.12976, 0.02501, 0.03048, // Ankle
1.00000, 0.00000, 0.00000, 0.00000
}; // Toes
MAvatarPostureValues values = new MAvatarPostureValues(desc.AvatarID, rotationValues);
skeleton.SetChannelData(values);
MVector3 pos = skeleton.GetGlobalJointPosition(desc.AvatarID, MJointType.S1L5Joint);
MVector3 gt = skeleton.GetRoot(desc.AvatarID).GetMJoint().Position;
gt = new MVector3(gt.X + rotationValues[0], gt.Y + rotationValues[1], gt.Z + rotationValues[2]);
System.Console.WriteLine("pos: {0}, {1}, {2}", pos.X, pos.Y, pos.Z);
System.Console.WriteLine("gt: {0}, {1}, {2}", gt.X, gt.Y, gt.Z);
Assert.IsTrue(System.Math.Abs(pos.X - gt.X) < 0.001);
Assert.IsTrue(System.Math.Abs(pos.Y - gt.Y) < 0.001);
Assert.IsTrue(System.Math.Abs(pos.Z - gt.Z) < 0.001);
}
/// <summary>
/// MMU causes problems if initializing multiple times -> To check in future
/// Basic initialization
/// For specifying the priorities of the MMUs /motion types the properties can be specified (e.g. {"walk", 1.0}, {"grasp", 2.0})
/// The listed motion types are also the ones which are loaded. If this porperty is not defined then every MMU is loaded.
/// </summary>
/// <param name="avatarDescription"></param>
/// <param name="properties"></param>
/// <returns></returns>
public override MBoolResponse Initialize(MAvatarDescription avatarDescription, Dictionary <string, string> properties)
{
base.Initialize(avatarDescription, properties);
Console.WriteLine("---------------------------------------------------------");
Console.WriteLine("Initializing co-simulation MMU");
//Full scene transmission initial required
this.transmitFullScene = true;
MSkeletonAccess.Iface SkeletonAccess = new IntermediateSkeleton();
SkeletonAccess.InitializeAnthropometry(avatarDescription);
//Setup the mmu access
this.mmuAccess = new MMUAccess(this.sessionId)
{
SkeletonAccess = SkeletonAccess,
SceneAccess = this.SceneAccess
};
Console.WriteLine("Try to connect to mmu access...");
//Connect to mmu access and load mmus
if (this.mmuAccess.Connect(this.AdapterEndpoint, avatarDescription.AvatarID))
{
//Get all loadable MMUs within the current session
List <MMUDescription> loadableMMUs = this.mmuAccess.GetLoadableMMUs();
//Create a dictionary for storing the priorities
Dictionary <string, float> priorities = new Dictionary <string, float>();
//Select the MMUs to load if explictely specified by the user
if (properties != null && properties.Count > 0)
{
for (int i = loadableMMUs.Count - 1; i >= 0; i--)
{
MMUDescription description = loadableMMUs[i];
float priority = 1.0f;
//If MMU is listed -> add the priority
if (priorities.TryGetValue(description.MotionType, out priority))
{
priorities.Add(description.MotionType, priority);
}
//MMU is not explicetly listed -> remove from loading list
else
{
loadableMMUs.RemoveAt(i);
}
}
}
//No MMU list defined -> Load all MMUs with same priority (despite the own MMU)
else
{
//Remove the own MMU -> Avoid recursively instantiating own MMU (unless explictely forced)
if (loadableMMUs.Exists(s => s.Name == this.Name))
{
MMUDescription ownDescription = loadableMMUs.Find(s => s.Name == this.Name);
loadableMMUs.Remove(ownDescription);
}
//Use the default priorities -> hacky
priorities = new Dictionary <string, float>()
{
{ "idle", 0 },
{ "walk", 1 },
{ "grasp", 2 },
{ "positionObject", 2 },
{ "releaseObject", 2 },
{ "release", 2 },
{ "move", 3 },
{ "carry", 2 },
{ "putDown", 2 },
{ "pickupMTM-SD", 2 }
};
}
Console.WriteLine("Got loadable MMUs:");
try
{
//Load the relevant MMUs
bool success = this.mmuAccess.LoadMMUs(loadableMMUs, TimeSpan.FromSeconds(10));
}
catch (Exception e)
{
Console.WriteLine("Error at loading MMUs : " + e.Message + e.StackTrace);
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
e.Message,
e.StackTrace
}
});
}
Console.WriteLine("All MMUs successfully loaded");
foreach (MMUDescription description in loadableMMUs)
{
Console.WriteLine(description.Name);
}
//Initialize all MMUs
bool initialized = this.mmuAccess.InitializeMMUs(TimeSpan.FromSeconds(10), avatarDescription.AvatarID);
if (!initialized)
{
Console.WriteLine("Problem at initializing MMUs");
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
{ "Problem at initializing MMUs" }
}
});
}
//Instantiate the cosimulator
this.coSimulator = new MMICoSimulator(mmuAccess.MotionModelUnits)
{
OverwriteSimulationState = true
};
//Set the priorities of the motions
this.coSimulator.SetPriority(priorities);
return(new MBoolResponse(true));
}
else
{
Console.WriteLine("Connection to MMUAccess/MMIRegister failed");
return(new MBoolResponse(false)
{
LogData = new List <string>()
{
"Connection to MMUAccess/MMIRegister failed"
}
});
}
}
