Пример #1
0
        /// <summary>
        /// Sets up the respective hand
        /// </summary>
        /// <param name="type"></param>
        /// <param name="instruction"></param>
        private void SetupHand(MJointType type, MInstruction instruction)
        {
            HandContainer hand = this.ActiveHands.Find(s => s.JointType == type);

            if (hand != null)
            {
                this.ActiveHands.Remove(hand);
            }


            //Create a new hand
            hand = new HandContainer(type, instruction, true);

            //First extract all parameters
            if (!instruction.Properties.GetValue(out hand.Velocity, "Velocity"))
            {
                hand.Velocity = 1.0f;
            }

            //First extract all parameters
            if (!instruction.Properties.GetValue(out hand.AngularVelocity, "AngularVelocity"))
            {
                hand.AngularVelocity = 30f;
            }

            if (!instruction.Properties.GetValue(out hand.Acceleration, "Acceleration"))
            {
                hand.Acceleration = 1.0f;
            }

            if (!instruction.Properties.GetValue(out hand.Repetitions, "Repetitions"))
            {
                hand.Repetitions = 1;
            }

            if (!instruction.Properties.GetValue(out hand.TurningAngle, "Angle"))
            {
                hand.TurningAngle = 45f;
            }

            if (instruction.Properties.GetValue(out hand.MinAngle, "MinAngle"))
            {
                hand.AngleIntervalDefined = true;
            }

            if (instruction.Properties.GetValue(out hand.MaxAngle, "MaxAngle"))
            {
                hand.AngleIntervalDefined = true;
            }


            instruction.Properties.GetValue(out hand.FixFingerTransformations, "FixFingerTransformations");

            if (instruction.Properties.ContainsKey("Axis"))
            {
                hand.TurningAxis = SceneAccess.GetTransformByID(instruction.Properties["Axis"]);
            }

            if (instruction.Properties.ContainsKey("SubjectID"))
            {
                hand.Subject = this.SceneAccess.GetSceneObjectByID(instruction.Properties["SubjectID"]);
            }

            if (instruction.Properties.ContainsKey("TargetID"))
            {
                hand.Target = this.SceneAccess.GetSceneObjectByID(instruction.Properties["TargetID"]);
            }


            this.ActiveHands.Add(hand);
        }
Пример #2
0
        /// <summary>
        /// Basic to step routine which computes the result of the current frame
        /// </summary>
        /// <param name="time"></param>
        /// <returns></returns>
        public override MSimulationResult DoStep(double time, MSimulationState simulationState)
        {
            //Create a new result
            MSimulationResult result = new MSimulationResult()
            {
                Events             = this.simulationState.Events ?? new List <MSimulationEvent>(),
                DrawingCalls       = new List <MDrawingCall>(),
                SceneManipulations = this.simulationState.SceneManipulations ?? new List <MSceneManipulation>(),
                Posture            = this.simulationState.Current,
                Constraints        = this.simulationState.Constraints ?? new List <MConstraint>()
            };

            List <MConstraint> constraints = result.Constraints;

            //Setup the constraint manager
            this.constraintManager.SetConstraints(ref constraints);

            //Set the simulation sate
            this.simulationState = simulationState;


            //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.Initialized)
                {
                    continue;
                }

                //Get the transform of the object to be positioned
                MTransform currentObjectTransform = hand.Subject.Transform;

                //Get the transform of the target
                MTransform targetObjectTransform = null;

                //Determine the next location of the object (at the end of the frame)
                MTransform nextObjectTransform = null;

                //Check if trajectory is defined
                if (hand.Trajectory != null)
                {
                    //The last point is the target transform
                    targetObjectTransform = hand.Trajectory.Last();


                    //Estimate the next transfom based on local motion planning
                    nextObjectTransform = this.DoLocalMotionPlanning(hand.Velocity, hand.AngularVelocity, TimeSpan.FromSeconds(time), currentObjectTransform.Position, currentObjectTransform.Rotation, hand.Trajectory[hand.TrajectoryIndex].Position, hand.Trajectory[hand.TrajectoryIndex].Rotation);


                    float  translationDistance = (nextObjectTransform.Position.Subtract(hand.Trajectory[hand.TrajectoryIndex].Position)).Magnitude();
                    double angularDistance     = MQuaternionExtensions.Angle(nextObjectTransform.Rotation, hand.Trajectory[hand.TrajectoryIndex].Rotation);


                    //Check if close to current target -> move to next target
                    if (translationDistance < 0.01f && angularDistance < 0.5f && hand.TrajectoryIndex < hand.Trajectory.Count - 1)
                    {
                        hand.TrajectoryIndex++;
                    }
                }

                //Default behavior if no trajectory is specified
                else
                {
                    targetObjectTransform = hand.Target.Transform;

                    //Estimate the next pose of the scene object
                    nextObjectTransform = this.DoLocalMotionPlanning(hand.Velocity, hand.AngularVelocity, TimeSpan.FromSeconds(time), currentObjectTransform.Position, currentObjectTransform.Rotation, targetObjectTransform.Position, targetObjectTransform.Rotation);
                }


                //Set the pose of the object to the next estimated pose
                result.SceneManipulations.Add(new MSceneManipulation()
                {
                    Transforms = new List <MTransformManipulation>()
                    {
                        new MTransformManipulation()
                        {
                            Target   = hand.Subject.ID,
                            Position = nextObjectTransform.Position,
                            Rotation = nextObjectTransform.Rotation
                        }
                    }
                });


                //Compute the next handpose based on the offset
                MTransform nextHandTransform = new MTransform("", nextObjectTransform.TransformPoint(hand.Offset.Position), nextObjectTransform.TransformRotation(hand.Offset.Rotation));


                //Set the ik constraints
                constraintManager.SetEndeffectorConstraint(new MJointConstraint(hand.JointType)
                {
                    GeometryConstraint = new MGeometryConstraint("")
                    {
                        ParentToConstraint = new MTransform(Guid.NewGuid().ToString(), nextHandTransform.Position, nextHandTransform.Rotation)
                    }
                });


                //To do add constraints
                float  distance    = (nextObjectTransform.Position.Subtract(targetObjectTransform.Position)).Magnitude();
                double angularDist = MQuaternionExtensions.Angle(nextObjectTransform.Rotation, targetObjectTransform.Rotation);

                if (hand.Trajectory != null && hand.TrajectoryIndex < hand.Trajectory.Count - 2)
                {
                    //Do nothing
                }
                else
                {
                    if (distance < 0.01f && angularDist < 0.5f)
                    {
                        //Increment the time
                        hand.ElapsedHoldTime += time;

                        if (hand.ElapsedHoldTime < hand.HoldTime)
                        {
                            continue;
                        }

                        this.ActiveHands.RemoveAt(i);

                        //Add new finished event
                        if (hand.BothHanded)
                        {
                            if (ActiveHands.Count == 0)
                            {
                                result.Events.Add(new MSimulationEvent(hand.Instruction.Name, mmiConstants.MSimulationEvent_End, hand.Instruction.ID));
                            }
                        }

                        //Single handed grasp
                        else
                        {
                            result.Events.Add(new MSimulationEvent(hand.Instruction.Name, mmiConstants.MSimulationEvent_End, hand.Instruction.ID));
                        }
                    }
                }
            }



            //Use the ik service
            if (result.Constraints.Count > 0)
            {
                MIKServiceResult ikResult = ServiceAccess.IKService.CalculateIKPosture(this.simulationState.Current, result.Constraints, new Dictionary <string, string>());

                result.Posture = ikResult.Posture;
            }

            return(result);
        }