public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState avatarState) { //Initialize the ik service this.ServiceAccess.IKService.Setup(this.AvatarDescription, new Dictionary <string, string>()); //Create a new constraint manager this.constraintManager = new ConstraintManager(this.SceneAccess); //Assign the instruction this.instruction = instruction; //Set state to ik this.state = ReleaseMotionState.IK; this.trajectoryIndex = 0; //Parse the parameters MBoolResponse response = this.ParseParameters(instruction); if (!response.Successful) { return(response); } return(new MBoolResponse(true)); }
/// <summary> /// Basic do step routine that is executed for each frame and generates the actual motion. /// </summary> /// <param name="time"></param> /// <param name="simulationState"></param> /// <returns></returns> private MSimulationResult DoStepIK(double time, MSimulationState simulationState) { //Create a default result MSimulationResult result = new MSimulationResult() { Events = new List <MSimulationEvent>(), Constraints = simulationState.Constraints ?? new List <MConstraint>(), SceneManipulations = new List <MSceneManipulation>(), Posture = simulationState.Current }; //The presently active constraints List <MConstraint> globalConstraints = result.Constraints; //The local constraints defined within the MMU List <MConstraint> localConstraints = new List <MConstraint>(); //Use the constraint manager to manage the local constraints constraintManager.SetConstraints(ref localConstraints); //Set the channel data to the approved state of the last frame (all MMUs were executed including the low prio grasp/positioning) this.SkeletonAccess.SetChannelData(simulationState.Initial); //Get the current hand position and rotation MVector3 currentHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint); MQuaternion currentHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint); //Set the skeleton acess data to the current this.SkeletonAccess.SetChannelData(simulationState.Current); //Determine the target hand position (either underlying MMU or given via boundary constraints) MTransform targetTransform = new MTransform() { Position = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint), Rotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint) }; MTransform nextPose = null; float translationWeight = 0; //Use the trajectory if defined if (this.trajectory != null && this.trajectory.Count > 0) { //Update the last element dynamically trajectory.Last().Position = targetTransform.Position; trajectory.Last().Rotation = targetTransform.Rotation; //Compute the next pose nextPose = this.DoLocalMotionPlanning(this.velocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, this.trajectory[trajectoryIndex].Position, this.trajectory[trajectoryIndex].Rotation, out translationWeight); //Check if close to current target -> move to next target -> To do consider rotation if ((nextPose.Position.Subtract(trajectory[trajectoryIndex].Position)).Magnitude() < 0.1f && MQuaternionExtensions.Angle(nextPose.Rotation, trajectory[trajectoryIndex].Rotation) < 1f && trajectoryIndex < trajectory.Count - 1) { trajectoryIndex++; } } else { //Compute the next pose nextPose = this.DoLocalMotionPlanning(this.velocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, targetTransform.Position, targetTransform.Rotation, out translationWeight); } ////Determine the next pose using local motion planning //MTransform nextPose = this.DoLocalMotionPlanning(this.velocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, targetHandPosition, targetHandRotation, out float translationWeight); //Perform a partial blend //result.Posture = this.PerformPartialBlend(this.handJoint, translationWeight, simulationState); //Get the current distance float currentDistance = (nextPose.Position.Subtract(targetTransform.Position)).Magnitude(); float currentAngularDistance = (float)MQuaternionExtensions.Angle(nextPose.Rotation, targetTransform.Rotation); //Handle the present state (either in ik mode oder blending) switch (this.state) { case ReleaseMotionState.IK: if (currentDistance < 0.02f && currentAngularDistance < 5f) { //Switch to blending to realize the final share this.state = ReleaseMotionState.Blending; this.elapsedBlendTime = 0; //Set to global constraints this.constraintManager.SetConstraints(ref globalConstraints); //Remove all constraints for the respective hand this.constraintManager.RemoveEndeffectorConstraints(this.handJoint); } else { //Update the constraint this.constraintManager.SetEndeffectorConstraint(this.handJoint, nextPose.Position, nextPose.Rotation); } break; } //Use the local constraint to compute the ik this.constraintManager.SetConstraints(ref localConstraints); //React depending on the given state switch (this.state) { //In ik mode the ik solver must be called case ReleaseMotionState.IK: //Create a list with the specific constraints for the reach MMU -> Only get the specific ones that must be solved (local constraints) List <MConstraint> ikConstraints = constraintManager.GetJointConstraints(); //Only solve if at least one constraint is defined if (ikConstraints.Count > 0) { //Compute twice MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, ikConstraints, new Dictionary <string, string>()); result.Posture = ikResult.Posture; } break; //In blending mode, motion blending must be performed case ReleaseMotionState.Blending: //Perform a blend elapsedBlendTime += (float)time; float blendWeight = Math.Min(1, elapsedBlendTime / endBlendDuration); result.Posture = MMICSharp.Common.Tools.Blending.PerformBlend(this.SkeletonAccess as IntermediateSkeleton, simulationState.Initial, simulationState.Current, blendWeight, true); //Perform a partial blend //result.Posture = this.PerformPartialBlend(this.handJoint, blendWeight, simulationState); if (blendWeight >= 1f) { result.Events.Add(new MSimulationEvent() { Name = "Release Finished", Reference = this.instruction.ID, Type = mmiConstants.MSimulationEvent_End }); } break; } //Combine the constraints this.constraintManager.SetConstraints(ref globalConstraints); this.constraintManager.Combine(localConstraints); return(result); }