/// <summary> /// Dynamically estimates the target transform /// </summary> /// <returns></returns> private MTransform ComputeTargetTransform(HandContainer hand) { MTransform target = new MTransform() { ID = "target" }; //Use the constraint (if defined) if (hand.Instruction.Constraints != null && hand.Instruction.Constraints.Exists(s => s.ID == hand.Instruction.Properties["TargetID"])) { MConstraint match = hand.Instruction.Constraints.Find(s => s.ID == hand.Instruction.Properties["TargetID"]); //Compute the global position and rotation of the geometry constraint target.Position = match.GeometryConstraint.GetGlobalPosition(this.SceneAccess); target.Rotation = match.GeometryConstraint.GetGlobalRotation(this.SceneAccess); } //Gather from the scene else { target = this.SceneAccess.GetTransformByID(hand.Instruction.Properties["TargetID"]); } return(target); }
/// <summary> /// Method is used to setup the parameters of one individual hand /// </summary> /// <param name="type"></param> /// <param name="instruction"></param> private void SetupHand(MJointType type, MInstruction instruction) { HandContainer hand = this.activeHands.Find(s => s.Type == type); if (hand != null) { this.activeHands.Remove(hand); } //Create a new hand hand = new HandContainer(type, instruction, true); if (instruction.Properties.ContainsKey("Velocity")) { hand.Velocity = float.Parse(instruction.Properties["Velocity"], System.Globalization.CultureInfo.InvariantCulture); } if (instruction.Properties.ContainsKey("AngularVelocity")) { hand.AngularVelocity = float.Parse(instruction.Properties["AngularVelocity"], System.Globalization.CultureInfo.InvariantCulture); } if (instruction.Properties.ContainsKey("Acceleration")) { hand.Acceleration = float.Parse(instruction.Properties["Acceleration"], System.Globalization.CultureInfo.InvariantCulture); } if (instruction.Properties.ContainsKey("HoldDuration")) { hand.HoldTime = float.Parse(instruction.Properties["HoldDuration"], System.Globalization.CultureInfo.InvariantCulture); } if (instruction.Properties.ContainsKey("CollisionAvoidance")) { hand.CollisionAvoidance = bool.Parse(instruction.Properties["CollisionAvoidance"]); if (hand.CollisionAvoidance) { MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, $"Using local collision avoidance, hand: {hand.Type}"); } } //First extract all parameters if (instruction.Properties.ContainsKey("Trajectory")) { string pathConstraintID = instruction.Properties["Trajectory"]; if (instruction.Constraints != null || instruction.Constraints.Where(s => s.PathConstraint != null && s.ID == pathConstraintID).Count() == 0) { //Get the path constraint MPathConstraint pathConstraint = instruction.Constraints.Find(s => s.PathConstraint != null && s.ID == pathConstraintID).PathConstraint; //Extract the trajectory hand.Trajectory = pathConstraint.GetMTransformList(); MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, $"Assigned hand trajectory. Number elements: {hand.Trajectory.Count}, hand: {hand.Type}"); } else { MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, $"Cannot assign trajectory of hand: {hand.Type}. No suitable MPathConstraint available."); } } 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 = simulationState.Events ?? new List <MSimulationEvent>(), DrawingCalls = new List <MDrawingCall>(), SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>(), Posture = simulationState.Current, Constraints = simulationState.Constraints ?? new List <MConstraint>() }; //The presently active constraints List <MConstraint> globalConstraints = new List <MConstraint>(result.Constraints); //The local constraints defined within the MMU List <MConstraint> localConstraints = new List <MConstraint>(); //Setup the constraint manager and use the local constraints this.constraintManager.SetConstraints(ref localConstraints); //Handle each active hand for (int i = this.activeHands.Count - 1; i >= 0; i--) { //Get the current hand HandContainer hand = this.activeHands[i]; //Skip if hand is not initialized if (!hand.Initialized) { continue; } //Get the transform of the object to be positioned MTransform currentObjectTransform = this.SceneAccess.GetTransformByID(hand.Instruction.Properties["SubjectID"]); //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(); //The current rajectory point MTransform currentTrajectoryPoint = hand.Trajectory[hand.TrajectoryIndex]; //Estimate the next transfom based on local motion planning nextObjectTransform = this.DoLocalMotionPlanning(hand.Velocity, hand.AngularVelocity, TimeSpan.FromSeconds(time), currentObjectTransform.Position, currentObjectTransform.Rotation, currentTrajectoryPoint.Position, currentTrajectoryPoint.Rotation, hand.CollisionAvoidance); //Get the current distance float currentDistance = nextObjectTransform.Position.Subtract(hand.Trajectory[hand.TrajectoryIndex].Position).Magnitude(); float currentAngularDistance = (float)MQuaternionExtensions.Angle(nextObjectTransform.Rotation, hand.Trajectory[hand.TrajectoryIndex].Rotation); //Check if close to current target -> move to next target if (currentDistance < this.translationThreshold && currentAngularDistance < this.rotationThreshold && hand.TrajectoryIndex < hand.Trajectory.Count - 1) { hand.TrajectoryIndex++; } } //Default behavior if no trajectory is specified else { targetObjectTransform = this.ComputeTargetTransform(hand); //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, hand.CollisionAvoidance); } //Set the pose of the object to the next estimated pose result.SceneManipulations.Add(new MSceneManipulation() { Transforms = new List <MTransformManipulation>() { new MTransformManipulation() { Target = hand.Instruction.Properties.GetValue("SubjectID", "subjectID"), 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(hand.Type, nextHandTransform.Position, nextHandTransform.Rotation); //To do add constraints float distance = (nextObjectTransform.Position.Subtract(targetObjectTransform.Position)).Magnitude(); float angularDistance = (float)MQuaternionExtensions.Angle(nextObjectTransform.Rotation, targetObjectTransform.Rotation); //Check if goal criteria fulfilled if (distance < this.translationThreshold && angularDistance < this.rotationThreshold) { //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)); } } } //Get the properties from the constraint manager List <MConstraint> jointConstraints = this.constraintManager.GetJointConstraints(); //Use the ik service if at least one constraint must be solved if (jointConstraints.Count > 0) { MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(simulationState.Current, jointConstraints, new Dictionary <string, string>()); result.Posture = ikResult.Posture; } //Configure the constraint manager to operate on the global constraints constraintManager.SetConstraints(ref globalConstraints); //Combine the global with the local ones constraintManager.Combine(localConstraints); //Provide the combined constraints as result result.Constraints = globalConstraints; //Return the simulation result return(result); }