/// <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); }