Esempio n. 1
0
        /// <summary>
        /// Basic do step call
        /// </summary>
        /// <param name="time"></param>
        /// <param name="simulationState"></param>
        /// <returns></returns>
        public override MSimulationResult DoStep(double time, MSimulationState simulationState)
        {
            //Transmit the scene (if first frame-> transmit full scene otherwise just deltas)
            this.mmuAccess.PushScene(this.transmitFullScene);

            //Full transmission only required at first frame
            this.transmitFullScene = false;

            //Perform the do step of the co-simulation
            MSimulationResult result = this.coSimulator.DoStep(time, simulationState);

            //Write the events
            if (result.Events != null && PrintDebugMessages)
            {
                foreach (MSimulationEvent ev in result.Events)
                {
                    Console.WriteLine("Event: " + ev.Name + " " + ev.Type + " " + ev.Reference);
                }
            }

            //Check the endcondition
            if (this.CheckEndCondition != null && this.CheckEndCondition(result))
            {
                result.Events.Add(new MSimulationEvent("Finished", mmiConstants.MSimulationEvent_End, this.Instruction.ID));
            }

            return(result);
        }
Esempio n. 2
0
        public override MSimulationResult DoStep(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
            };


            elapsed += (float)time;

            float blendWeight = Math.Min(1, elapsed / blendDuration);

            //Perform the actual motion blending
            result.Posture = MMICSharp.Common.Tools.Blending.PerformBlend(this.SkeletonAccess as IntermediateSkeleton, simulationState.Initial, simulationState.Current, blendWeight, true);


            //Provide end event if finished
            if (elapsed >= this.blendDuration)
            {
                result.Events.Add(new MSimulationEvent("Blend finished", mmiConstants.MSimulationEvent_End, this.instruction.ID));
            }


            return(result);
        }
Esempio n. 3
0
        /// <summary>
        /// Performs the actual solving
        /// </summary>
        /// <param name="input"></param>
        /// <param name="mmuResults"></param>
        /// <returns></returns>
        public virtual MSimulationResult Solve(MSimulationResult input, List <MSimulationResult> mmuResults, float timespan)
        {
            MSimulationResult result = input;

            //By default use all constraints
            List <MConstraint> constraints = new List <MConstraint>(input.Constraints);

            //Only solve the constraints which are not already fulfilled
            if (this.SolveViolatedConstraintsOnly)
            {
                constraints = this.GetViolatedIKConstraints(input.Constraints, result.Posture);
            }

            //Compute the ik if at least one constraint is not fulfilled
            if (constraints.Count > 0)
            {
                MAvatarPostureValues currentPosture = input.Posture;

                //Solve n times
                for (int i = 0; i < this.Iterations; i++)
                {
                    MIKServiceResult ikResult = this.serviceAccess.IKService.CalculateIKPosture(currentPosture, constraints, new Dictionary <string, string>());
                    currentPosture = ikResult.Posture;
                }

                result.Posture = currentPosture;
            }


            return(result);
        }
Esempio n. 4
0
        /// <summary>
        /// Method to compute a new posture for the next frame
        /// </summary>
        /// <param name="time"></param>
        /// <param name="simulationState"></param>
        /// <returns></returns>
        public override MSimulationResult DoStep(Double time, MSimulationState simulationState)
        {
            //Create a result to store all the computed data
            MSimulationResult result = new MSimulationResult()
            {
                //Just forward the present constraints
                Constraints        = simulationState.Constraints ?? new List <MConstraint>(),
                Posture            = simulationState.Current,
                SceneManipulations = simulationState.SceneManipulations
            };


            //Execute instructions on main thread
            this.ExecuteOnMainThread(() =>
            {
                this.SkeletonAccess.SetChannelData(simulationState.Current);

                this.AssignPostureValues(simulationState.Current);

                //this.transform.position = this.SkeletonAccess.GetRootPosition(this.AvatarDescription.AvatarID).ToVector3();
                //this.transform.rotation = this.SkeletonAccess.GetRootRotation(this.AvatarDescription.AvatarID).ToQuaternion();

                this.animator.Update((float)time);

                result.Posture = this.GetRetargetedPosture();
            });

            return(result);
        }
        /// <summary>
        /// Checks whether solving is required
        /// </summary>
        /// <param name="result"></param>
        /// <param name="timeSpan"></param>
        /// <returns></returns>
        public bool RequiresSolving(MSimulationResult result, float timeSpan)
        {
            List <MJointConstraint> jointConstraints = new List <MJointConstraint>();

            foreach (MConstraint constraint in result.Constraints)
            {
                if (constraint.JointConstraint != null)
                {
                    jointConstraints.Add(constraint.JointConstraint);
                }

                if (constraint.PostureConstraint != null && constraint.PostureConstraint.JointConstraints != null)
                {
                    jointConstraints.AddRange(constraint.PostureConstraint.JointConstraints);
                }
            }


            foreach (MJointConstraint constraint in jointConstraints)
            {
                //Skip if joint is not considered
                if (!this.ConsideredTypes.Contains(constraint.JointType))
                {
                    continue;
                }

                //To do integrate further checks
            }

            return(true);
        }
 /// <summary>
 /// Assign the posture after compute frame
 /// </summary>
 /// <param name="result"></param>
 public override void PostComputeFrame(MSimulationResult result)
 {
     if (!this.ComputeAsync)
     {
         this.avatar.AssignPostureValues(result.Posture);
     }
 }
Esempio n. 7
0
        /// <summary>
        /// Constructor for a mmu container which is initialized by serialized data
        /// </summary>
        /// <param name="data"></param>
        /// <param name="mmus"></param>
        /// <param name="tasks"></param>
        public MMUContainer(SerializableMMUContainer data, List <IMotionModelUnitAccess> mmus, List <MotionTask> tasks)
        {
            this.History = new List <MotionTask>();

            if (data.History != null)
            {
                foreach (string id in data.History)
                {
                    MotionTask task = tasks.Find(s => s.ID == id);

                    if (task != null)
                    {
                        this.History.Add(task);
                    }
                }
            }


            this.IsActive     = data.IsActive;
            this.LastResult   = data.LastResult;
            this.LastResults  = data.LastResults;
            this.MMU          = mmus.Find(s => s.ID == data.MMUID);
            this.Priority     = data.Priority;
            this.CurrentTasks = new List <MotionTask>();


            if (tasks != null)
            {
                foreach (string currentTaskId in data.CurrentTaskIDs)
                {
                    MotionTask match = tasks.Find(s => s.ID == currentTaskId);
                    this.CurrentTasks.Add(match);
                }
            }
        }
Esempio n. 8
0
        /// <summary>
        /// Do step method which computes the actual carry postures
        /// </summary>
        /// <param name="time"></param>
        /// <param name="simulationState"></param>
        /// <returns></returns>
        public override MSimulationResult DoStep(double time, MSimulationState simulationState)
        {
            //Set the current avatar state
            this.simulationState = 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>()
            };

            //Special case for both handed carry
            if (bothHandedCarry)
            {
                this.DoStepBothHanded(result, time, simulationState);
            }

            //Single handed carry ->Perform a single handed do step
            else
            {
                result = this.DoStepSingleHanded(result, time, simulationState);
            }


            //Return the computed result
            return(result);
        }
Esempio n. 9
0
        public override MSimulationResult DoStep(double time, MSimulationState simulationState)
        {
            MSimulationResult result = new MSimulationResult()
            {
                Constraints        = simulationState.Constraints,
                Events             = simulationState.Events ?? new List <MSimulationEvent>(),
                Posture            = simulationState.Current,
                SceneManipulations = simulationState.SceneManipulations,
                DrawingCalls       = new List <MDrawingCall>()
            };


            this.elapsed += TimeSpan.FromSeconds(time);

            //Add a drwaing call
            result.DrawingCalls.Add(new MDrawingCall(MDrawingCallType.DrawText)
            {
                Properties = new Dictionary <string, string>()
                {
                    { "text", this.text + this.elapsed.TotalSeconds + " s" }
                }
            });


            //Create the finished event
            if (this.elapsed > this.duration)
            {
                result.Events.Add(new MSimulationEvent("Finished", mmiConstants.MSimulationEvent_End, this.instruction.ID));
            }


            return(result);
        }
        /// <summary>
        /// Method responsible for the inspector visualization
        /// </summary>
        public override void OnInspectorGUI()
        {
            //Get the cosimulation debugger instance
            CoSimulationDebugger debugger = this.target as CoSimulationDebugger;

            if (debugger.SourceAvatar == null)
            {
                GUILayout.Label("Source avatar must be set in order to use the CoSimulationDebugger.");

                //Call the base inspector
                base.OnInspectorGUI();

                return;
            }


            //Check if the present frame is not null
            if (debugger.currentFrame != null)
            {
                GUILayout.Label("Hierachy:");
                EditorGUILayout.LabelField("", GUI.skin.horizontalSlider);

                //Draw the initial
                this.DrawInitial(debugger.currentHierachyIndex == 0);

                int index = 0;
                for (int i = 0; i < debugger.currentFrame.Results.Count; i++)
                {
                    MSimulationResult result      = debugger.currentFrame.Results[i];
                    MInstruction      instruction = debugger.GetInstructionByID(debugger.currentFrame.Instructions[i]);


                    DrawHierachyEntry(debugger, result, instruction, index + 1);
                    index++;
                }

                GUI.color = Color.blue;

                for (int i = 0; i < debugger.currentFrame.CoSimulationSolverResults.Count; i++)
                {
                    MSimulationResult result = debugger.currentFrame.CoSimulationSolverResults[i];


                    DrawSolverEntry(debugger, result, index + 1);
                    index++;
                }

                GUI.color = Color.cyan;

                //Draw the merged resuls
                this.DrawMergedResult(debugger, debugger.currentFrame.MergedResult, debugger.currentHierachyIndex == -1);

                GUI.color = GUI.color = Color.white;
            }

            //Call the base inspector
            base.OnInspectorGUI();
        }
Esempio n. 11
0
        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>()
            };

            List <MConstraint> constraints = new List <MConstraint>();


            //Apply ik
            if (LeftHandTarget != null)
            {
                constraints.Add(new MConstraint(System.Guid.NewGuid().ToString())
                {
                    JointConstraint = new MJointConstraint()
                    {
                        GeometryConstraint = new MGeometryConstraint("")
                        {
                            ParentToConstraint = new MTransform(System.Guid.NewGuid().ToString(), LeftHandTarget.Transform.Position, LeftHandTarget.Transform.Rotation),
                            WeightingFactor    = 1.0f,
                        },
                        JointType = MJointType.LeftWrist
                    }
                });
            }

            if (RightHandTarget != null)
            {
                constraints.Add(new MConstraint(System.Guid.NewGuid().ToString())
                {
                    JointConstraint = new MJointConstraint()
                    {
                        GeometryConstraint = new MGeometryConstraint("")
                        {
                            ParentToConstraint = new MTransform(System.Guid.NewGuid().ToString(), RightHandTarget.Transform.Position, RightHandTarget.Transform.Rotation),
                            WeightingFactor    = 1.0f
                        },
                        JointType = MJointType.RightWrist
                    },
                });
            }

            if (constraints.Count > 0)
            {
                MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(simulationState.Current, constraints, new Dictionary <string, string>());
                result.Posture = ikResult.Posture;
            }

            return(result);
        }
Esempio n. 12
0
        /// <summary>
        /// Stores the mmu result within the container
        /// </summary>
        /// <param name="result"></param>
        /// <param name="maxQueueLength"></param>
        public void StoreResult(MSimulationResult result, int maxQueueLength = 3)
        {
            this.LastResult = result;

            this.LastResults.Enqueue(result);

            while (this.LastResults.Count > maxQueueLength)
            {
                this.LastResults.Dequeue();
            }
        }
Esempio n. 13
0
        /// <summary>
        /// Method is repsonsible for modeling the actual carry for both handed objects
        /// </summary>
        /// <param name="result"></param>
        private void BothHandedCarry(ref MSimulationResult result)
        {
            //Create an empty transform representing the next object transform
            MTransform nextObjectTransform = new MTransform();


            //Update the desired object transform
            if (this.CarryTargetName != null && this.CarryTargetName.Length > 0)
            {
                MTransform targetTransform = SceneAccess.GetTransformByID(this.CarryTargetName);
                nextObjectTransform.Position = targetTransform.Position;
                nextObjectTransform.Rotation = targetTransform.Rotation;
            }
            else
            {
                MTransform refTransform = GetTransform(this.simulationState.Initial, bothHandedCarryReferenceJoint);
                MVector3   forward      = GetRootForwad(this.simulationState.Initial);
                //Determine the ref transform rotation
                refTransform.Rotation = MQuaternionExtensions.FromEuler(new MVector3(0, Extensions.SignedAngle(new MVector3(0, 0, 1), forward, new MVector3(0, 1, 0)), 0));

                nextObjectTransform.Position = refTransform.TransformPoint(this.internalCarryTransform.Position);
                nextObjectTransform.Rotation = refTransform.TransformRotation(this.internalCarryTransform.Rotation);
            }



            //Compute the object transform
            result.SceneManipulations.Add(new MSceneManipulation()
            {
                Transforms = new List <MTransformManipulation>()
                {
                    new MTransformManipulation()
                    {
                        Target   = instruction.Properties["TargetID"],
                        Position = nextObjectTransform.Position,
                        Rotation = nextObjectTransform.Rotation
                    }
                }
            });


            List <MIKProperty> ikProperties = new List <MIKProperty>();


            //Update the hands
            foreach (HandContainer hand in this.ActiveHands)
            {
                //Update the hands
                MTransform nextHandPose = new MTransform("", nextObjectTransform.TransformPoint(hand.HandOffset.Position), nextObjectTransform.TransformRotation(hand.HandOffset.Rotation));

                this.constraintManager.SetEndeffectorConstraint(hand.JointType, nextHandPose.Position, nextHandPose.Rotation, hand.ConstraintID);
            }
        }
        /// <summary>
        /// Performs a simulation cycle for a single frame
        /// </summary>
        /// <param name="time"></param>
        protected virtual void DoStepAsync(float time)
        {
            //##################### Scene synchronization of each MMU Access ####################################################
            this.PushScene();

            //##################### Do the Co Simulation for each Avatar ####################################################

            //Create a dictionary which contains the avatar states for each MMU
            ConcurrentDictionary <MMIAvatar, MSimulationResult> results = new ConcurrentDictionary <MMIAvatar, MSimulationResult>();


            //Compute the frame in parallel manner
            foreach (MMIAvatar avatar in this.Avatars)
            {
                MSimulationResult result = avatar.CoSimulator.DoStep(time, new MSimulationState()
                {
                    Current = avatar.LastPosture,
                    Initial = avatar.LastPosture
                });

                //Assign the resulting posture
                avatar.LastPosture = result.Posture;

                //Execute additional method which can be implemented by child class
                this.OnResultComputed(result, avatar);

                results.TryAdd(avatar, result);
            }

            //Increment the frame number
            this.frameNumber++;


            //####################### Incorporate Results ##################################################

            //Perform on main thread
            MainThreadDispatcher.Instance.ExecuteBlocking(() =>
            {
                //Apply the manipulations of the scene
                this.ApplySceneUpdate(results);

                //Update the physics
                this.UpdatePhysics(time);

                //Assign the posture of each avatar
                foreach (MMIAvatar avatar in this.Avatars)
                {
                    avatar.AssignPostureValues(avatar.LastPosture.Copy());
                }
            });
        }
        public override MSimulationResult DoStep(double time, MSimulationState simulationState)
        {
            //The simulation result which is provided as overall result
            MSimulationResult result = new MSimulationResult()
            {
                Posture            = simulationState.Current,
                Events             = new List <MSimulationEvent>(),
                SceneManipulations = new List <MSceneManipulation>()
            };

            //Handle each active MMU (each instruction coressponds to one MMU)
            for (int i = instructions.Count - 1; i >= 0; i--)
            {
                //Update the simulation state
                MSimulationResult localResult = mmuInstances[instructions[i]].DoStep(time, simulationState);

                //Update the simulation state
                //simulationState.Current = localResult.Posture;

                //Just forward the constraints
                simulationState.Constraints = localResult.Constraints;

                //Write the result
                result.Constraints = localResult.Constraints;
                result.Posture     = localResult.Posture;

                //Merge the scene manipulations
                result.SceneManipulations?.AddRange(localResult.SceneManipulations);

                //Add the events
                if (localResult.Events != null && localResult.Events.Count > 0)
                {
                    result.Events.AddRange(localResult.Events);
                }

                //Merge the drawing calls
                result.DrawingCalls?.AddRange(localResult.DrawingCalls);

                if (localResult.Events.Exists(s => s.Type == mmiConstants.MSimulationEvent_End && s.Reference == instructions[i].ID))
                {
                    //Remove the respective MMU
                    mmuInstances.Remove(instructions[i]);

                    //Remove from the list
                    instructions.RemoveAt(i);
                }
            }

            return(result);
        }
        private InstructionWindow GetInstructionWindow(MInstruction instruction)
        {
            InstructionWindow window = new InstructionWindow()
            {
                StartIndex  = -1,
                EndIndex    = -1,
                Instruction = instruction
            };

            for (int i = 0; i < this.record.Frames.Count; i++)
            {
                CoSimulationFrame frame = this.record.Frames[i];

                if (frame.Instructions.Contains(instruction.ID))
                {
                    if (window.StartIndex == -1)
                    {
                        window.StartIndex = i;
                    }


                    if (window.EndIndex == -1 || window.EndIndex < i)
                    {
                        window.EndIndex = i;
                    }


                    MSimulationResult result = frame.MergedResult;

                    if (result.Events != null && result.Events.Count > 0)
                    {
                        foreach (MSimulationEvent simEvent in result.Events)
                        {
                            if (simEvent.Reference == instruction.ID)
                            {
                                window.Events.Add(i);
                                window.RawEvents.Add(simEvent);
                                break;
                            }
                        }
                    }
                }
            }



            return(window);
        }
Esempio n. 17
0
        //Callback which is called for each frame
        private void CoSimulator_OnResult(object sender, MSimulationResult e)
        {
            //Aquire the mutex during the writing of the new results
            this.simulationEventMutex.WaitOne();

            currentEvents.Events         = new List <MSimulationEvent>(e.Events);
            currentEvents.FrameNumber    = (int)this.coSimulator.FrameNumber;
            currentEvents.SimulationTime = this.coSimulator.Time;
            MAvatarPostureValues postureValues = e.Posture.Copy();

            //Add the data
            data.TryAdd(currentEvents.FrameNumber, currentEvents);

            this.simulationEventMutex.ReleaseMutex();


            //Perform in a new thread
            Task.Run(() =>
            {
                //Handle each event type which is registered
                foreach (var entry in callbackClients)
                {
                    //Handle each client
                    foreach (CoSimulationEventCallbackClient client in entry.Value)
                    {
                        if (entry.Key == "OnFrameEnd")
                        {
                            client.Access.OnFrameEnd(postureValues);
                        }

                        //Check if events available
                        if (currentEvents.Events != null)
                        {
                            var relevantEvents = currentEvents.Events.Where(s => s.Type == entry.Key).ToList();

                            if (relevantEvents.Count > 0)
                            {
                                client.Access.OnEvent(new MCoSimulationEvents(relevantEvents, currentEvents.SimulationTime, currentEvents.FrameNumber));
                            }
                        }
                    }
                }
            });
        }
        /// <summary>
        /// Performs the actual solving
        /// </summary>
        /// <param name="currentResult"></param>
        /// <param name="mmuResults"></param>
        /// <param name="timeSpan"></param>
        /// <returns></returns>
        public MSimulationResult Solve(MSimulationResult currentResult, List <MSimulationResult> mmuResults, float timeSpan)
        {
            List <MJointConstraint> jointConstraints = new List <MJointConstraint>();

            foreach (MConstraint constraint in currentResult.Constraints)
            {
                if (constraint.JointConstraint != null)
                {
                    jointConstraints.Add(constraint.JointConstraint);
                }

                if (constraint.PostureConstraint != null && constraint.PostureConstraint.JointConstraints != null)
                {
                    jointConstraints.AddRange(constraint.PostureConstraint.JointConstraints);
                }
            }

            //Set the channel data
            this.skeletonAccess.SetChannelData(currentResult.Posture);

            foreach (MJointConstraint constraint in jointConstraints)
            {
                //Skip if joint is not considered
                if (!this.ConsideredTypes.Contains(constraint.JointType))
                {
                    continue;
                }


                MGeometryConstraint geometryConstraint = constraint.GeometryConstraint;


                //To do -> Further check the parent
                if (geometryConstraint.ParentToConstraint != null)
                {
                    this.skeletonAccess.SetLocalJointPosition(currentResult.Posture.AvatarID, constraint.JointType, geometryConstraint.ParentToConstraint.Position);
                    this.skeletonAccess.SetLocalJointRotation(currentResult.Posture.AvatarID, constraint.JointType, geometryConstraint.ParentToConstraint.Rotation);
                }
            }

            currentResult.Posture = this.skeletonAccess.RecomputeCurrentPostureValues(currentResult.Posture.AvatarID);

            return(currentResult);
        }
Esempio n. 19
0
        /// <summary>
        /// Performs the do step realted computations for both handed mode
        /// </summary>
        /// <param name="result"></param>
        /// <param name="time"></param>
        /// <param name="simulationState"></param>
        /// <returns></returns>
        private MSimulationResult DoStepBothHanded(MSimulationResult result, double time, MSimulationState simulationState)
        {
            //The presently active constraints
            List <MConstraint> globalConstraints = result.Constraints;

            //Operate on the local constraints
            this.constraintManager.SetConstraints(ref globalConstraints);


            switch (this.bothHandedState)
            {
            //First position the object in order to carry it properly
            case CarryState.Positioning:

                //Perform both handed positioning
                this.PositionObjectBothHanded(ref result, time);

                //Solve using ik if constraints are defined
                if (this.constraintManager.GetJointConstraints().Count > 0)
                {
                    MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, this.constraintManager.GetJointConstraints(), new Dictionary <string, string>());
                    result.Posture = ikResult.Posture;
                }

                break;

            case CarryState.Carry:

                //Perform a both handed carry (just preserve the relative position and rotation)
                this.BothHandedCarry(ref result);

                //Solve using ik if constraints are defined
                if (this.constraintManager.GetJointConstraints().Count > 0)
                {
                    MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, this.constraintManager.GetJointConstraints(), new Dictionary <string, string>());
                    result.Posture = ikResult.Posture;
                }

                break;
            }

            return(result);
        }
Esempio n. 20
0
        /// <summary>
        /// Performs the do step realted computations for the single hand mode
        /// </summary>
        /// <param name="result"></param>
        /// <param name="time"></param>
        /// <param name="simulationState"></param>
        /// <returns></returns>
        private MSimulationResult DoStepSingleHanded(MSimulationResult result, double time, MSimulationState simulationState)
        {
            //The presently active constraints
            List <MConstraint> globalConstraints = result.Constraints;

            //Operate on the local constraints
            this.constraintManager.SetConstraints(ref globalConstraints);


            //Handle each active hand
            for (int i = this.ActiveHands.Count - 1; i >= 0; i--)
            {
                //Get the current hand container
                HandContainer hand = this.ActiveHands[i];

                //Handle the state
                switch (hand.State)
                {
                case CarryState.Positioning:
                    //Call positioning of single hand and add the resulting ik properties (e.g. hand position/Rotation)
                    this.PositionObjectSingleHand(ref result, time, hand);
                    break;

                case CarryState.Carry:
                    //Call the single handed carry and add the resulting ik properties (e.g. hand position/rotation)
                    this.SingleHandedCarry(ref result, time, hand);
                    break;
                }
            }


            //Get the joint constraints
            List <MConstraint> jointConstraints = this.constraintManager.GetJointConstraints();

            //Solve using ik if constraints are defined
            if (jointConstraints.Count > 0)
            {
                MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, jointConstraints, new Dictionary <string, string>());
                result.Posture = ikResult.Posture;
            }

            return(result);
        }
        /// <summary>
        /// Basic do step call
        /// </summary>
        /// <param name="time"></param>
        /// <param name="simulationState"></param>
        /// <returns></returns>
        public override MSimulationResult DoStep(double time, MSimulationState simulationState)
        {
            //Transmit the scene (if first frame-> transmit full scene otherwise just deltas)
            this.mmuAccess.PushScene(this.transmitFullScene);

            //Full transmission only required at first frame
            this.transmitFullScene = false;

            //Perform the do step of the co-simulation
            MSimulationResult result = this.coSimulator.DoStep(time, simulationState);

            if (result.Events != null)
            {
                foreach (MSimulationEvent ev in result.Events)
                {
                    Console.WriteLine("Event: " + ev.Name + " " + ev.Type + " " + ev.Reference);
                }
            }

            return(result);
        }
Esempio n. 22
0
        public SerializableMMUContainer(MMUContainer container)
        {
            if (container.CurrentTasks != null)
            {
                foreach (MotionTask task in container.CurrentTasks)
                {
                    this.CurrentTaskIDs.Add(task.ID);
                }
            }

            this.ID = container.ID;


            if (container.History != null)
            {
                this.History = container.History.Select(s => s.ID).ToList();
            }
            this.IsActive    = container.IsActive;
            this.LastResult  = container.LastResult;
            this.LastResults = container.LastResults;
            this.MMUID       = container.MMU.ID;
            this.Priority    = container.Priority;
        }
Esempio n. 23
0
        public override MSimulationResult DoStep(double time, MSimulationState avatarState)
        {
            //Call the remote cosimulation
            MSimulationResult result = this.remoteCoSimulationMMU.DoStep(time, avatarState);

            //Fire events
            if (result != null && result.Events != null && result.Events.Count > 0)
            {
                foreach (MSimulationEvent simEvent in result.Events)
                {
                    this.MSimulationEventHandler?.Invoke(this, simEvent);
                }
            }

            try
            {
                this.avatar.AssignPostureValues(result.Posture);
            }
            catch (Exception)
            {
                Debug.LogError("Problem assigning posture using remote co-simulation");
            }
            return(result);
        }
Esempio n. 24
0
        /// <summary>
        /// Performs a single handed carry
        /// Just sets the object relative to the hand
        /// </summary>
        /// <param name="result"></param>
        /// <param name="hand"></param>
        /// <returns></returns>
        private List <MIKProperty> SingleHandedCarry(ref MSimulationResult result, double time, HandContainer hand)
        {
            List <MIKProperty> ikProperties = new List <MIKProperty>();

            //Check if a carry target is specified
            if (hand.CarryTargetName != null)
            {
                //Compute the root velocity
                double rootVelocity = this.ComputeRootVelocity(time);

                //Get the current transform of the carry target for the respective hand
                MTransform targetTr = SceneAccess.GetTransformByID(this.CarryTargetName);

                //The transform of the carry target
                MTransform targetTransform = new MTransform("", targetTr.Position, targetTr.Rotation);

                //Compute the global position of the respective hand based on the object
                MVector3    targetHandPosition = targetTransform.TransformPoint(hand.HandOffset.Position);
                MQuaternion targetHandRotation = targetTransform.TransformRotation(hand.HandOffset.Rotation);

                //Get the current hand transform
                MTransform currentHandTransform = GetTransform(simulationState.Initial, hand.Type);

                //Compute the new hand pose
                MTransform nextHandPose = this.DoLocalMotionPlanning(rootVelocity + hand.Velocity, TimeSpan.FromSeconds(time), currentHandTransform.Position, currentHandTransform.Rotation, targetHandPosition, targetHandRotation);

                //Compute the object transform
                result.SceneManipulations.Add(new MSceneManipulation()
                {
                    Transforms = new List <MTransformManipulation>()
                    {
                        new MTransformManipulation()
                        {
                            Target   = hand.Instruction.Properties["TargetID"],
                            Position = nextHandPose.TransformPoint(hand.ObjectOffset.Position),
                            Rotation = nextHandPose.TransformRotation(hand.ObjectOffset.Rotation)
                        }
                    }
                });

                //Set the position and rotation parameters of the ik
                this.constraintManager.SetEndeffectorConstraint(hand.JointType, nextHandPose.Position, nextHandPose.Rotation, hand.ConstraintID);
            }

            //Just set the object relative to the current hand
            else
            {
                //Create a transform representing the hand transform for the planned frame
                MTransform handTransform = GetTransform(simulationState.Current, hand.Type);

                if (this.UseCarryIK)
                {
                    this.constraintManager.SetEndeffectorConstraint(hand.JointType, handTransform.Position, handTransform.Rotation, hand.ConstraintID);
                }



                //Compute the object transform
                result.SceneManipulations.Add(new MSceneManipulation()
                {
                    Transforms = new List <MTransformManipulation>()
                    {
                        new MTransformManipulation()
                        {
                            Target   = hand.Instruction.Properties["TargetID"],
                            Position = handTransform.TransformPoint(hand.ObjectOffset.Position),
                            Rotation = handTransform.TransformRotation(hand.ObjectOffset.Rotation)
                        }
                    }
                });
            }


            //To do optionally consider self-collisions
            return(ikProperties);
        }
Esempio n. 25
0
        /// <summary>
        /// Performs the position step for a single hand
        /// </summary>
        /// <param name="result"></param>
        /// <param name="time"></param>
        /// <param name="hand"></param>
        /// <returns></returns>
        private void PositionObjectSingleHand(ref MSimulationResult result, double time, HandContainer hand)
        {
            //Compute the root velocity
            double rootVelocity = this.ComputeRootVelocity(time);

            //The current hand transform (the approved result of the last frame)
            MTransform currentHandTransform = GetTransform(simulationState.Initial, hand.Type);

            //The desired hand transform (of the underlying animation)
            MTransform targetHandTransform = GetTransform(simulationState.Current, hand.Type);


            //Check if for the hand a carry target is defined
            if (hand.CarryTargetName != null)
            {
                //Get the target transform if a carry target is defined
                MTransform carryTargetTransform = SceneAccess.GetTransformByID(this.CarryTargetName);

                //Compute the global position of the respective hand based on the object
                targetHandTransform.Position = carryTargetTransform.TransformPoint(hand.HandOffset.Position);
                targetHandTransform.Rotation = carryTargetTransform.TransformRotation(hand.HandOffset.Rotation);
            }


            rootVelocity = 0f;

            //Compute the new hand pose at the end of this frame
            MTransform nextHandTransform = this.DoLocalMotionPlanning(rootVelocity + hand.Velocity, TimeSpan.FromSeconds(time), currentHandTransform.Position, currentHandTransform.Rotation, targetHandTransform.Position, targetHandTransform.Rotation);


            //Compute the object transform
            result.SceneManipulations.Add(new MSceneManipulation()
            {
                Transforms = new List <MTransformManipulation>()
                {
                    new MTransformManipulation()
                    {
                        Target = hand.Instruction.Properties["TargetID"],
                        //Compute the object location with respect to the offset
                        Position = nextHandTransform.TransformPoint(hand.ObjectOffset.Position),
                        Rotation = nextHandTransform.TransformRotation(hand.ObjectOffset.Rotation)
                    }
                }
            });


            float translationDistance = targetHandTransform.Position.Subtract(nextHandTransform.Position).Magnitude();
            float angularDistance     = (float)MQuaternionExtensions.Angle(nextHandTransform.Rotation, targetHandTransform.Rotation);

            //Check if goal reached -> change state
            if (translationDistance < 0.01f && angularDistance < 2)
            {
                result.Events.Add(new MSimulationEvent("PositioningFinished", "PositioningFinished", hand.Instruction.ID));

                //Finally in carry state
                hand.State = CarryState.Carry;

                //Set the constraint if the carry ik is enabled
                if (UseCarryIK || hand.CarryTargetName != null)
                {
                    //Remove the endeffector constraint no carry ik
                    this.constraintManager.SetEndeffectorConstraint(hand.JointType, nextHandTransform.Position, nextHandTransform.Rotation, hand.ConstraintID);
                }
                else
                {
                    //Remove the endeffector constraint no carry ik
                    this.constraintManager.RemoveEndeffectorConstraints(hand.JointType);
                }
            }

            //Not finished
            else
            {
                //Set the position and rotation parameters of the ik
                this.constraintManager.SetEndeffectorConstraint(hand.JointType, nextHandTransform.Position, nextHandTransform.Rotation, hand.ConstraintID);
            }
        }
Esempio n. 26
0
        /// <summary>
        /// Method is responsible for modeling the positiong the object and hands which is the first part of the carry for both handed objects
        /// </summary>
        /// <param name="result"></param>
        /// <param name="time"></param>
        private void PositionObjectBothHanded(ref MSimulationResult result, double time)
        {
            double rootVelocity = this.ComputeRootVelocity(time);


            MAvatarPostureValues avatarPose             = this.simulationState.Initial;
            MTransform           currentObjectTransform = this.SceneAccess.GetTransformByID(this.instruction.Properties["TargetID"]);


            //Move the object to a central spot in front of the avatar
            //Create a new transform for the target object transform
            MTransform targetObjectTransform = new MTransform();


            if (this.CarryTargetName != null && this.CarryTargetName.Length > 0)
            {
                MTransform targetTransform = SceneAccess.GetTransformByID(this.CarryTargetName);
                targetObjectTransform.Position = targetTransform.Position;
                targetObjectTransform.Rotation = targetTransform.Rotation;
            }

            else
            {
                MTransform refTransform = GetTransform(this.simulationState.Initial, bothHandedCarryReferenceJoint);
                MVector3   forward      = GetRootForwad(this.simulationState.Initial);
                //Determine the ref transform rotation
                refTransform.Rotation = MQuaternionExtensions.FromEuler(new MVector3(0, Extensions.SignedAngle(new MVector3(0, 0, 1), forward, new MVector3(0, 1, 0)), 0));

                targetObjectTransform.Position = refTransform.TransformPoint(this.internalCarryTransform.Position);
                targetObjectTransform.Rotation = refTransform.TransformRotation(this.internalCarryTransform.Rotation);
            }

            MTransform nextObjectPose      = this.DoLocalMotionPlanning(rootVelocity + positionObjectVelocity, TimeSpan.FromSeconds(time), currentObjectTransform.Position, currentObjectTransform.Rotation, targetObjectTransform.Position, targetObjectTransform.Rotation);
            MTransform nextObjectTransform = new MTransform("", nextObjectPose.Position, nextObjectPose.Rotation);


            //Update the position of the object
            result.SceneManipulations.Add(new MSceneManipulation()
            {
                Transforms = new List <MTransformManipulation>()
                {
                    new MTransformManipulation()
                    {
                        Target   = instruction.Properties["TargetID"],
                        Position = nextObjectPose.Position,
                        Rotation = nextObjectPose.Rotation
                    }
                }
            });

            //Update the hands
            foreach (HandContainer hand in this.ActiveHands)
            {
                //Update the hands
                MTransform nextHandPose = new MTransform("", nextObjectTransform.TransformPoint(hand.HandOffset.Position), nextObjectTransform.TransformRotation(hand.HandOffset.Rotation));

                //Set a new endeffector constraint
                this.constraintManager.SetEndeffectorConstraint(hand.JointType, nextHandPose.Position, nextHandPose.Rotation, hand.ConstraintID);

                //Assign the hand pose to preserve finger rotations
                result.Posture = AssignHandPose(result.Posture, hand.HandPose, hand.Type);
            }



            //Check if position is finished
            if ((targetObjectTransform.Position.Subtract(nextObjectPose.Position)).Magnitude() < 0.01f && MQuaternionExtensions.Angle(targetObjectTransform.Rotation, nextObjectPose.Rotation) < 0.1f)
            {
                result.Events.Add(new MSimulationEvent("PositioningFinished", "PositioningFinished", instruction.ID));

                //Only consider the rotation around y axis
                double yRotation = this.GetRootRotation(this.simulationState.Initial).ToEuler().Y;

                MTransform rootTransform = new MTransform("", this.GetRootPosition(this.simulationState.Initial), MQuaternionExtensions.FromEuler(new MVector3(0, yRotation, 0)));

                //Update the new relative coordinates
                this.relativeObjectRotation = rootTransform.InverseTransformRotation(nextObjectTransform.Rotation);
                this.relativeObjectPosition = rootTransform.InverseTransformPoint(nextObjectTransform.Position);

                this.bothHandedState = CarryState.Carry;

                //Get the joint constraints
                List <MConstraint> jointConstraints = this.constraintManager.GetJointConstraints();

                //Solve using ik if constraints are defined
                if (jointConstraints.Count > 0)
                {
                    MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, jointConstraints, new Dictionary <string, string>());
                    result.Posture = ikResult.Posture;
                }
            }
        }
Esempio n. 27
0
        /// <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);
        }
Esempio n. 28
0
        /// <summary>
        /// Do step routine in which the actual simulation result is generated
        /// </summary>
        /// <param name="time"></param>
        /// <param name="simulationState"></param>
        /// <returns></returns>
        public override MSimulationResult DoStep(double time, MSimulationState simulationState)
        {
            //Create a new simulation result
            MSimulationResult result = new MSimulationResult()
            {
                Events             = new List <MSimulationEvent>(),
                Constraints        = simulationState.Constraints ?? new List <MConstraint>(),
                SceneManipulations = new List <MSceneManipulation>(),
                Posture            = simulationState.Current
            };

            //Compute the target transform at the beginning of each frame
            this.targetTransform = this.ComputeTargetTransform();

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


            //The next pose
            MTransform nextPose = null;

            //The current velocity used for path planning
            float currentVelocity = this.velocity;// + this.ComputeRootVelocity(time, simulationState);

            //Use the trajectory if defined
            if (this.trajectory != null)
            {
                //If a trajectory is used -> The target transform is the last point of the trajectory
                this.targetTransform = this.trajectory.Last();

                //Compute the next pose
                nextPose = this.DoLocalMotionPlanning(currentVelocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, this.trajectory[trajectoryIndex].Position, this.trajectory[trajectoryIndex].Rotation);

                //Check if close to current target -> move to next target -> To do consider rotation
                if ((nextPose.Position.Subtract(trajectory[trajectoryIndex].Position)).Magnitude() < this.translationThreshold && MQuaternionExtensions.Angle(nextPose.Rotation, trajectory[trajectoryIndex].Rotation) < this.rotationThreshold && trajectoryIndex < trajectory.Count - 1)
                {
                    trajectoryIndex++;
                }
            }


            else
            {
                //Compute the next pose
                nextPose = this.DoLocalMotionPlanning(currentVelocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, this.targetTransform.Position, this.targetTransform.Rotation);
            }


            //Get the current distance
            float currentDistance        = (nextPose.Position.Subtract(targetTransform.Position)).Magnitude();
            float currentAngularDistance = (float)MQuaternionExtensions.Angle(nextPose.Rotation, targetTransform.Rotation);


            //Check if the ik is only computed once and blending is performed subsequently
            if (this.singleShotIK)
            {
                //Estimate the weight for blending
                float weight = (float)Math.Min(1, (currentVelocity * time) / currentDistance);

                //To check -> Why is a deep copy required?
                result.Posture = Blending.PerformBlend((IntermediateSkeleton)this.SkeletonAccess, simulationState.Initial, this.singleShotIKTargetPosture.Copy(), weight, false);


                if (weight >= 1 - 1e-3)
                {
                    result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));

                    constraintManager.SetEndeffectorConstraint(new MJointConstraint(this.handJoint)
                    {
                        GeometryConstraint = new MGeometryConstraint()
                        {
                            ParentObjectID     = "",
                            ParentToConstraint = new MTransform(System.Guid.NewGuid().ToString(), targetTransform.Position, targetTransform.Rotation)
                        }
                    });
                }
            }

            //Default scenario -> IK is computed for each frame
            else
            {
                if (currentDistance <= this.translationThreshold && currentAngularDistance <= this.rotationThreshold)
                {
                    //Set the target
                    nextPose.Position = targetTransform.Position;
                    nextPose.Rotation = targetTransform.Rotation;

                    MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, "Reach finished");
                    result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
                }

                //Set the desired endeffector constraints
                constraintManager.SetEndeffectorConstraint(this.handJoint, nextPose.Position, nextPose.Rotation);
            }


            //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)
            {
                int ikIterations = 1;

                MIKServiceResult ikResult = null;

                //Use the ik to compute a posture fulfilling the requested constraints
                //To do -> Try with different initial postures / compute suitability of the generated posture
                for (int i = 0; i < ikIterations; i++)
                {
                    //Compute twice
                    ikResult       = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, ikConstraints, new Dictionary <string, string>());
                    result.Posture = ikResult.Posture;
                }
            }

            //Update the constraint manager to operate on the global constraints
            constraintManager.SetConstraints(ref globalConstraints);

            //Integrate the newly defined constraints in the global ones
            constraintManager.Combine(localConstraints);

            //Just for better understanding -> Assign the previous constraints + integrated ones to the result (this is not neccessary since the constraint manager is operating on the reference)
            result.Constraints = globalConstraints;

            //Return the result
            return(result);
        }
Esempio n. 29
0
        /// <summary>
        /// Do step routine in which the actual simulation result is generated
        /// </summary>
        /// <param name="time"></param>
        /// <param name="simulationState"></param>
        /// <returns></returns>
        public override MSimulationResult DoStep(double time, MSimulationState simulationState)
        {
            //Create a new simulation result
            MSimulationResult result = new MSimulationResult()
            {
                Events             = new List <MSimulationEvent>(),
                Constraints        = simulationState.Constraints ?? new List <MConstraint>(),
                SceneManipulations = new List <MSceneManipulation>()
            };

            this.SkeletonAccess.SetChannelData(simulationState.Current.Copy());

            // Target position we want to transform to
            MVector3 targetPos = this.targetTransform.Position;

            // Fully body posture. Only Pelvis Center (first joint) has to be manipulated.
            List <double> posture = simulationState.Current.PostureData;

            // Current position and distance to target.
            MVector3 currentPos = this.SkeletonAccess.GetRootPosition(simulationState.Initial.AvatarID);
            MVector3 distance   = targetPos.Subtract(currentPos);

            // Current rotation and rotational diff to target
            MQuaternion currentRot = this.SkeletonAccess.GetRootRotation(simulationState.Initial.AvatarID);
            MQuaternion targetRot  = this.targetTransform.Rotation;



            MVector3 newPos;

            MVector3 deltaDistance = distance.Clone();


            if (this.velocity > 0)
            {
                deltaDistance = distance.Normalize().Multiply(this.velocity * time);
                Console.WriteLine("Delta v: " + deltaDistance.Magnitude() + " " + time + " " + this.velocity);
            }



            // If no velocity set or distance very close, directly morph to target position and rotation.
            if (this.velocity <= 0 || distance.Magnitude() < deltaDistance.Magnitude())
            {
                newPos = targetPos;

                // Set rotation
                //posture[3] = this.targetTransform.Rotation.W;
                //posture[4] = this.targetTransform.Rotation.X;
                //posture[5] = this.targetTransform.Rotation.Y;
                //posture[6] = this.targetTransform.Rotation.Z;

                // Add end event.
                Console.WriteLine("Finished with vel " + this.velocity + " at " + distance.Magnitude());
                result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
            }
            else // if velocity > 0 and distance sufficiently large, we should apply linear translation with the provided velocity.
            {
                newPos = currentPos.Add(deltaDistance);
                Console.WriteLine("Target Location: " + this.targetTransform.Position + " " + currentPos + " " + distance + " " + deltaDistance + " " + newPos);
            }

            Console.WriteLine("newposrot: " + newPos + " " + targetRot);
            this.SkeletonAccess.SetRootPosition(simulationState.Current.AvatarID, newPos);
            this.SkeletonAccess.SetRootRotation(simulationState.Current.AvatarID, targetRot);

            result.Posture = this.SkeletonAccess.GetCurrentPostureValues(simulationState.Current.AvatarID);

            Console.WriteLine("Frame : " + result.Posture.PostureData[0] + " " + result.Posture.PostureData[1] + " " + result.Posture.PostureData[2] + " " + result.Posture.PostureData[3] + " " + result.Posture.PostureData[4] + " " + result.Posture.PostureData[5] + " " + result.Posture.PostureData[6] + " ");

            //Return the result
            return(result);
        }
Esempio n. 30
0
        /// <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 DoStepBlending(double time, MSimulationState simulationState)
        {
            //Create a new simulation result
            MSimulationResult result = new MSimulationResult()
            {
                Events             = simulationState.Events ?? new List <MSimulationEvent>(),
                Constraints        = simulationState.Constraints,
                SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>(),
                Posture            = simulationState.Current
            };


            //Directly operate on the global constraints -> since no ik is computed
            List <MConstraint> globalConstraints = result.Constraints;

            //Assign the global constraints to the constraint manager
            this.constraintManager.SetConstraints(ref globalConstraints);

            //Use the initial state (approved posture of last frame)
            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);


            MJointConstraint jointConstraint = null;

            switch (this.handJoint)
            {
            case MJointType.LeftWrist:
                jointConstraint = this.constraintManager.GetEndeffectorConstraint(MJointType.LeftWrist);
                break;

            case MJointType.RightWrist:
                jointConstraint = this.constraintManager.GetEndeffectorConstraint(MJointType.RightWrist);
                break;
            }


            //Handle the joint constraint
            if (jointConstraint != null)
            {
                //Get the current hand positon based on the constraint
                currentHandPosition = jointConstraint.GeometryConstraint.GetGlobalPosition(this.SceneAccess);
                currentHandRotation = jointConstraint.GeometryConstraint.GetGlobalRotation(this.SceneAccess);
            }

            //Set the skeleton to the current state
            this.SkeletonAccess.SetChannelData(simulationState.Current);


            //Determine the target hand position (either underlying MMU or given via boundary constraints)
            MVector3    targetHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint);
            MQuaternion targetHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint);

            //Move the hand from the current position to the target position
            MVector3 deltaPosition = targetHandPosition.Subtract(currentHandPosition);

            float angle = (float)MQuaternionExtensions.Angle(currentHandRotation, targetHandRotation);

            //Compute the distance of the hand to the target hand position
            float distanceToGoal = deltaPosition.Magnitude();

            //Compute the max distance which can be covered within the current frame
            float maxDistance = (float)(time * this.velocity);

            //Compute the max allowed angle
            float maxAngle = (float)(time * this.angularVelocity);

            //Compute the weight for slerping (weight increases with shrinking distance to target)
            float translationWeight = Math.Min(1.0f, maxDistance / distanceToGoal);

            //Compute the rotation weight
            float rotationWeight = Math.Min(1.0f, maxAngle / angle);

            //Blend from the current rotation to the target
            result.Posture = Blending.PerformBlend((IntermediateSkeleton)this.SkeletonAccess, simulationState.Initial, simulationState.Current, Math.Min(translationWeight, rotationWeight), true);

            this.SkeletonAccess.SetChannelData(result.Posture);

            if (distanceToGoal < 0.01f)
            {
                result.Events.Add(new MSimulationEvent()
                {
                    Name      = "Release Finished",
                    Reference = this.instruction.ID,
                    Type      = mmiConstants.MSimulationEvent_End
                });

                //Remove all constraints for the respective hand
                this.constraintManager.RemoveEndeffectorConstraints(this.handJoint);
            }
            else
            {
                //Remove the endeffector constraint
                this.constraintManager.RemoveEndeffectorConstraints(this.handJoint);

                //Update the constraint
                this.constraintManager.SetEndeffectorConstraint(this.handJoint, this.SkeletonAccess.GetGlobalJointPosition(result.Posture.AvatarID, this.handJoint), this.SkeletonAccess.GetGlobalJointRotation(result.Posture.AvatarID, this.handJoint));
            }



            return(result);
        }