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