/// <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> /// To do in future use instruction as argument /// Because it is not know which instruction should be aborted /// </summary> /// <returns></returns> public override MBoolResponse Abort(string instructionID) { //To do -> identify the correct hand which should be aborted HandContainer hand = this.ActiveHands.Find(s => s.Instruction.ID == instructionID); if (hand != null) { this.ActiveHands.Remove(hand); } else { Console.WriteLine("Carry aborted"); this.ActiveHands.Clear(); } return(new MBoolResponse(true)); }
/// <summary> /// Setups the hand /// </summary> /// <param name="type"></param> /// <param name="instruction"></param> /// <returns></returns> private HandContainer SetupHand(HandType 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) { BlendStartPosture = this.simulationState.Initial.Copy(), ElapsedBlendTime = 0, CarryTargetName = null }; //Compute the blend time based on the distance and velocity hand.BlendDuration = GetHandDistance(hand.Type) / hand.Velocity; //First extract all parameters if (instruction.Properties.ContainsKey("Velocity")) { hand.Velocity = float.Parse(instruction.Properties["Velocity"], System.Globalization.CultureInfo.InvariantCulture); } //Use the carry target if defined if (instruction.Properties.ContainsKey("CarryTarget")) { hand.CarryTargetName = instruction.Properties["CarryTarget"]; } this.ActiveHands.Add(hand); return(hand); }
/// <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); }
/// <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); } }