/// <summary>
/// Method sets up the retargeting of the specified avaatar using an initial posture
/// </summary>
/// <param name="id"></param>
/// <param name="reference"></param>
/// <returns></returns>
public virtual MAvatarPosture SetupRetargeting(string id, MAvatarPosture reference)
{
this.AvatarID = id;
Animator anim = this.GetComponent <Animator>();
this.animatorReference = anim;
if (reference != null)
{
this.bonenameMap = new Dictionary <string, MJointType>();
foreach (MJoint j in reference.Joints)
{
if (!this.bonenameMap.ContainsKey(j.ID))
{
this.bonenameMap.Add(j.ID, j.Type);
}
}
}
if (this.bonenameMap == null)
{
this.bonenameMap = new Dictionary <string, MJointType>();
foreach (MJointType b in humanbonemap.Keys)
{
bonenameMap.Add(anim.GetBoneTransform(humanbonemap[b]).name, b);
}
}
MAvatarPosture p;
if (reference != null)
{
retargetingService.SetupRetargeting(reference);
skeleton = retargetingService.GetSkeleton();
p = reference;
}
else
{
p = this.GenerateGlobalPosture();
retargetingService.SetupRetargeting(p);
skeleton = retargetingService.GetSkeleton();
}
//Create an empty joint object if not defined
if (this.gameJointPrefab == null)
{
this.gameJointPrefab = new GameObject("emptyJoint");
}
if (UseSkeletonVisualization)
{
skelVis = new SkeletonVisualization(skeleton, retargetingService, this.Pelvis, this.bonenameMap, this.AvatarID, gameJointPrefab);
}
this.AssignPostureValues(this.GetPosture());
return(p);
}
/// <summary>
/// Performs a blending based on the from posture and the to posture. In particular a blending weight and an additional blending mask is utilized. If the blending mask is set to null, all bones with position + rotation will be used for blending.
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="weight"></param>
/// <param name="blendingMask"></param>
/// <returns></returns>
public static MAvatarPostureValues PerformBlend(IntermediateSkeleton skeleton, MAvatarPostureValues from, MAvatarPostureValues to, float weight, Dictionary <MJointType, BlendProperty> blendingMask = null)
{
//MAvatarPosture result = from.Clone();
MAvatarPosture zero = skeleton.GetAvatarDescription(from.AvatarID).ZeroPosture;
skeleton.SetChannelData(from);
List <MQuaternion> fromRot = skeleton.GetLocalJointRotations(from.AvatarID);
skeleton.SetChannelData(to);
List <MQuaternion> toRot = skeleton.GetLocalJointRotations(to.AvatarID);
for (int i = 0; i < zero.Joints.Count; i++)
{
//By default belnd both position and rotation
BlendProperty blendProperty = new BlendProperty(1.0f, 1.0f);
MJointType joint = zero.Joints[i].Type;
if (blendingMask != null && blendingMask.ContainsKey(joint))
{
//Get the bone weight
blendingMask.TryGetValue(joint, out blendProperty);
}
//Perform a linear interpolation of the position
// Does not correspond to intermediate skeleton representation.
// result.Joints[i].Position = result.Joints[i].Position.Lerp(to.Joints[i].Position, weight * blendProperty.PositionWeight);
//Perform a slerp of the rotation
skeleton.SetLocalJointRotation(to.AvatarID, joint, fromRot[i].Slerp(toRot[i], weight * blendProperty.RotationWeight));
}
return(skeleton.RecomputeCurrentPostureValues(to.AvatarID));
}
public SkeletonVisualization(IntermediateSkeleton skeleton, RetargetingService retargetingService, Transform RootBone, Dictionary <string, MJointType> bonenameMap, string AvatarID, GameObject gameJointPrefab)
{
//this.anim = anim;
this.RootBone = RootBone;
this.bonenameMap = bonenameMap.Invert();
this.skeleton = skeleton;
this.AvatarID = AvatarID;
this.retargetingService = retargetingService;
root = new ISVisualizationJoint((RJoint)skeleton.GetRoot(this.AvatarID), RootBone, this.bonenameMap);
//root.AlignAvatar(anim);
root.CreateGameObjSkel(gameJointPrefab);
//root.SetToZero(this.AvatarID);
}
/// <summary>
/// Basic constructor
/// </summary>
public TransitionModeler(IntermediateSkeleton skeleton)
{
this.elapsedTime = 0f;
this.blendTime = 1.0f;
this.Finished = false;
this.Active = false;
//Set default to root transform and rotation
this.Mask = BlendingMask.RootTransformAndRotations;
//Set default linear weight function
this.WeightFunction = (float time) =>
{
return(this.elapsedTime / this.blendTime);
};
this.Skeleton = skeleton;
}
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]);
}
}
/// <summary>
/// Performs a blending based on the from posture and the to posture.
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="weight"></param>
/// <param name="rootTransform">Specifies whether the root transform is blended as well</param>
/// <returns></returns>
public static MAvatarPostureValues PerformBlend(IntermediateSkeleton skeleton, MAvatarPostureValues from, MAvatarPostureValues to, float weight, bool rootTransform = true)
{
MAvatarPosture zero = skeleton.GetAvatarDescription(from.AvatarID).ZeroPosture;
skeleton.SetChannelData(from);
List <MQuaternion> fromRot = skeleton.GetLocalJointRotations(from.AvatarID);
skeleton.SetChannelData(to);
List <MQuaternion> toRot = skeleton.GetLocalJointRotations(to.AvatarID);
for (int i = 0; i < zero.Joints.Count; i++)
{
//By default belnd both position and rotation
MJointType joint = zero.Joints[i].Type;
//Perform a linear interpolation of the position
// Does not correspond to intermediate skeleton representation.
// result.Joints[i].Position = result.Joints[i].Position.Lerp(to.Joints[i].Position, weight * blendProperty.PositionWeight);
//Perform a slerp of the rotation
skeleton.SetLocalJointRotation(to.AvatarID, joint, fromRot[i].Slerp(toRot[i], weight));
}
return(skeleton.RecomputeCurrentPostureValues(to.AvatarID));
/*
* MAvatarPosture result = from.Clone();
*
* for (int i = 0; i < result.Joints.Count; i++)
* {
* //Skip if root transform should be ignored
* if (i == 0 && rootTransform)
* result.Joints[i].Position = result.Joints[i].Position.Lerp(to.Joints[i].Position, weight);
*
* //Perform a slerp of the rotation
* result.Joints[i].Rotation = result.Joints[i].Rotation.Slerp(to.Joints[i].Rotation, weight);
* }
*
* return result;
*/
}
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"
}
});
}
}
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new result
MSimulationResult result = new MSimulationResult()
{
Events = simulationState.Events ?? new List <MSimulationEvent>(),
DrawingCalls = new List <MDrawingCall>(),
SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>(),
Posture = simulationState.Current,
Constraints = simulationState.Constraints ?? new List <MConstraint>()
};
//Cast to intermediate skeleton to get all functions
IntermediateSkeleton iS = this.SkeletonAccess as IntermediateSkeleton;
//Assign the approved posture of the last frame (proper finger rotations)
this.SkeletonAccess.SetChannelData(simulationState.Initial);
//First estimate the finger rotations
//------------------------------------------------------------------------------------------------------------------
//Handle each active hand
for (int i = this.ActiveHands.Count - 1; i >= 0; i--)
{
//Get the current hand
HandContainer hand = this.ActiveHands[i];
if (hand.Release)
{
//Use all finger joint if in release mode
foreach (MJointType jointType in fingerJoints)
{
//Get the current rotation of the finger
if (!hand.CurrentFingerRotations.ContainsKey(jointType))
{
hand.CurrentFingerRotations.Add(jointType, iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, jointType));
}
else
{
hand.CurrentFingerRotations[jointType] = iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, jointType);
}
}
}
else
{
//Handle all joint constraints -> Use only the fingers that are constrained
foreach (MJointConstraint joint in hand.FinalPosture.JointConstraints)
{
//Skip if the joint is no finger joint
if (!this.IsFingerJoint(joint.JointType))
{
continue;
}
//Get the current rotation of the finger
if (!hand.CurrentFingerRotations.ContainsKey(joint.JointType))
{
hand.CurrentFingerRotations.Add(joint.JointType, iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, joint.JointType));
}
else
{
hand.CurrentFingerRotations[joint.JointType] = iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, joint.JointType);
}
}
}
}
//Perform the blending
//------------------------------------------------------------------------------------------------------------------
//Assign the approved posture of the last frame (proper finger rotations)
this.SkeletonAccess.SetChannelData(simulationState.Current);
//Handle each active hand
for (int i = this.ActiveHands.Count - 1; i >= 0; i--)
{
//Get the current hand
HandContainer hand = this.ActiveHands[i];
//Flag which indicates whether the fingers are positioned
bool positioned = true;
if (hand.Release)
{
//If in release mode again use all joints being finger joints
foreach (MJointType jointType in fingerJoints)
{
//Get the current rotation of the finger
MQuaternion currentRotation = hand.CurrentFingerRotations[jointType];
//The current rotation is the result of the last frame
MQuaternion desiredRotation = iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, jointType);
//The weight used for blending
double weight = 0;
//The angle between the current rotation and the desired one
double angle = MQuaternionExtensions.Angle(currentRotation, desiredRotation);
//Compute the weight based on the elapsed time if the duration is set
if (hand.HasDuration)
{
weight = hand.Elapsed / hand.Duration;
}
//By default use the angular velocity
else
{
//The max allowed angle in this frame
double maxAngle = time * hand.AngularVelocity;
weight = Math.Min(1, maxAngle / angle);
}
//Compute the new rotation
MQuaternion newRotation = MQuaternionExtensions.Slerp(currentRotation, desiredRotation, (float)weight);
//Set the local joint rotation of the intermediate skeleton
iS.SetLocalJointRotation(this.AvatarDescription.AvatarID, jointType, newRotation);
//Goal criteria
if (angle > 0.1f)
{
positioned = false;
}
}
}
else
{
//Handle all joint constraints
foreach (MJointConstraint joint in hand.FinalPosture.JointConstraints)
{
//Skip if the joint is no finger joint
if (!this.IsFingerJoint(joint.JointType))
{
continue;
}
//Get the current rotation of the finger
MQuaternion currentRotation = hand.CurrentFingerRotations[joint.JointType];
//Get the desired rotation
MQuaternion desiredRotation = joint.GeometryConstraint.ParentToConstraint.Rotation;
//The weight used for blending
double weight = 0;
//The angle between the current rotation and the desired one
double angle = MQuaternionExtensions.Angle(currentRotation, desiredRotation);
//Compute the weight based on the elapsed time if the duration is set
if (hand.HasDuration)
{
weight = hand.Elapsed / hand.Duration;
}
//By default use the angular velocity
else
{
//The max allowed angle in this frame
double maxAngle = time * hand.AngularVelocity;
weight = Math.Min(1, maxAngle / angle);
}
//Compute the new rotation
MQuaternion newRotation = MQuaternionExtensions.Slerp(currentRotation, desiredRotation, (float)weight);
//Set the local joint rotation of the intermediate skeleton
iS.SetLocalJointRotation(this.AvatarDescription.AvatarID, joint.JointType, newRotation);
//Goal criteria
if (angle > 0.1f)
{
positioned = false;
}
}
}
//Provide event if positioned successfully
if (positioned && !hand.Positioned)
{
hand.Positioned = true;
result.Events.Add(new MSimulationEvent()
{
Name = "MoveFingersMMU",
Type = "FingersPositioned",
Reference = hand.Instruction.ID
});
}
//Increment the time
hand.Elapsed += (float)time;
}
//Recompute the posture given the performed changes
result.Posture = iS.RecomputeCurrentPostureValues(this.AvatarDescription.AvatarID);
//Return the simulation result for the given frame
return(result);
}
