/// <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; } } }
public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState) { //Reset the properties this.UseCarryIK = false; this.bothHandedCarry = false; this.bothHandedState = CarryState.None; this.simulationState = simulationState; //Create a list which contains the hands which should be considered for carrying List <HandContainer> toPlan = new List <HandContainer>(); if (instruction.Properties == null) { throw new Exception($"{this.Name}: No properties defined!"); } //Extract the hand information if (instruction.Properties.ContainsKey("Hand")) { switch (instruction.Properties["Hand"]) { case "Left": toPlan.Add(this.SetupHand(HandType.Left, instruction)); this.bothHandedCarry = false; break; case "Right": toPlan.Add(this.SetupHand(HandType.Right, instruction)); this.bothHandedCarry = false; break; case "Both": //Set flag for both handed carry this.bothHandedCarry = true; toPlan.Add(this.SetupHand(HandType.Left, instruction)); toPlan.Add(this.SetupHand(HandType.Right, instruction)); break; } } else { toPlan.Add(this.SetupHand(HandType.Right, instruction)); } //Use the carry target if defined if (!instruction.Properties.GetValue(out this.CarryTargetName, "CarryTarget")) { this.CarryTargetName = null; } //Use the carry target if defined if (!instruction.Properties.GetValue(out this.UseCarryIK, "UseCarryIK")) { UseCarryIK = false; } //Use carry distance if defined if (!instruction.Properties.GetValue(out this.carryDistanceBothHanded, "CarryDistance")) { carryDistanceBothHanded = 0.65f; } //Use carry distance if defined if (!instruction.Properties.GetValue(out this.carryHeightBothHanded, "CarryHeight")) { carryHeightBothHanded = 0.2f; } //Use carry distance if defined if (!instruction.Properties.GetValue(out this.positionObjectVelocity, "Velocity")) { this.positionObjectVelocity = 1.0f; } //Compute and plan the relevant aspects of each hand foreach (HandContainer hand in toPlan) { //Get the (initial) hand transform MTransform handTransform = this.GetTransform(simulationState.Initial, hand.Type); //Get the current transform of the scene object MTransform sceneObjectTransform = this.SceneAccess.GetTransformByID(hand.Instruction.Properties["TargetID"]); //Get the hand pose try { hand.HandPose = GetTransform(simulationState.Initial, hand.Type); } catch (Exception e) { Console.WriteLine("Problem estimating hand pose: " + e.Message + e.StackTrace); } //Compute the relative transform of the hand (hand relative to object) hand.HandOffset = new MTransform("", sceneObjectTransform.InverseTransformPoint(handTransform.Position), sceneObjectTransform.InverseTransformRotation(handTransform.Rotation)); //Compute the inverse offset (object relative to hand) hand.ObjectOffset = new MTransform("", handTransform.InverseTransformPoint(sceneObjectTransform.Position), handTransform.InverseTransformRotation(sceneObjectTransform.Rotation)); //Set state to positioning hand.State = CarryState.Positioning; } //Do additional computations for both handed carry if (bothHandedCarry) { //Set the state to positioning this.bothHandedState = CarryState.Positioning; //Assign the instruction this.instruction = instruction; //Get the current object transorm MTransform currentObjectTransform = this.SceneAccess.GetTransformByID(this.instruction.Properties["TargetID"]); //Get the current root transform -> to do MTransform rootTransform = GetTransform(this.simulationState.Initial, MJointType.PelvisCentre); //Compute the relative object transform this.relativeObjectRotation = rootTransform.InverseTransformRotation(currentObjectTransform.Rotation); this.relativeObjectPosition = rootTransform.InverseTransformPoint(currentObjectTransform.Position); //Manually specify a carry target if (this.CarryTargetName == null || this.CarryTargetName.Length == 0) { MTransform refTransform = GetTransform(this.simulationState.Initial, bothHandedCarryReferenceJoint); MVector3 forward = GetRootForwad(this.simulationState.Initial); //Determine the ref transform rotation just consider the y axis rotation refTransform.Rotation = MQuaternionExtensions.FromEuler(new MVector3(0, Extensions.SignedAngle(new MVector3(0, 0, 1), forward, new MVector3(0, 1, 0)), 0)); //Compute the delta //MVector3 delta = currentObjectTransform.Position.Subtract(refTransform.Position); //MVector3 direction = new MVector3(delta.X, 0, delta.Z).Normalize(); //The carry position i MVector3 carryPosition = refTransform.Position.Add(forward.Multiply(this.carryDistanceBothHanded)).Add(new MVector3(0, carryHeightBothHanded, 0f)); //Forwad + offset this.internalCarryTransform = new MTransform("CarryTarget", refTransform.InverseTransformPoint(carryPosition), refTransform.InverseTransformRotation(currentObjectTransform.Rotation)); } } return(new MBoolResponse(true)); }
//[MParameterAttribute("fixFingerTransformations", "bool", "Specifies whether the finger locations should be fixed", false)] public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState) { this.simulationState = simulationState; //Extract the hand information if (instruction.Properties.ContainsKey("Hand")) { switch (instruction.Properties["Hand"]) { case "Left": this.SetupHand(MJointType.LeftWrist, instruction); break; case "Right": this.SetupHand(MJointType.RightWrist, instruction); break; case "Both": this.SetupHand(MJointType.LeftWrist, instruction); this.SetupHand(MJointType.RightWrist, instruction); break; } } else { this.SetupHand(MJointType.RightWrist, instruction); } //To fix -> might overwrite already active hands foreach (HandContainer hand in this.ActiveHands) { if (hand.Trajectory == null && !hand.Instruction.Properties.ContainsKey("TargetID") && hand.TurningAxis == null) { MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, "At least one of both trajectory or target must be set. Aborting Move MMU"); return(new MBoolResponse(false)); } //hand.Instruction.Properties.Contains("Repetitions") MVector3 handPosition = GetGlobalPosition(simulationState.Initial, hand.JointType); MQuaternion handRotation = GetGlobalRotation(simulationState.Initial, hand.JointType); //Get the transform of the scene object to be moved MTransform sceneObjectTransform = this.SceneAccess.GetTransformByID(hand.Instruction.Properties["SubjectID"]); //Compute the relative transform of the hand hand.Offset = new MTransform("", sceneObjectTransform.InverseTransformPoint(handPosition), sceneObjectTransform.InverseTransformRotation(handRotation)); hand.Initialized = true; hand.Trajectory = new List <MTransform>(); if (hand.TurningAxis != null) { //Get the initial transform of the subject MTransform initialTransform = hand.Subject.Transform.Clone(); //Determine the final transform MTransform finalTransform = null; if (hand.AngleIntervalDefined) { MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, $"Called Turning MMU with turning axis defined. Min angle {hand.MinAngle}, max angle {hand.MaxAngle}."); finalTransform = RotateAround(initialTransform, hand.TurningAxis.Position, hand.TurningAxis.Rotation.Multiply(new MVector3(0, 0, 1)), hand.MinAngle); finalTransform = RotateAround(initialTransform, hand.TurningAxis.Position, hand.TurningAxis.Rotation.Multiply(new MVector3(0, 0, 1)), hand.MaxAngle); } else { MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, $"Called Turning MMU with turning axis defined. Turning angle {hand.TurningAngle}."); finalTransform = RotateAround(initialTransform, hand.TurningAxis.Position, hand.TurningAxis.Rotation.Multiply(new MVector3(0, 0, 1)), hand.TurningAngle); } for (int i = 0; i < hand.Repetitions; i++) { hand.Trajectory.Add(finalTransform); hand.Trajectory.Add(initialTransform); } } else { for (int i = 0; i < hand.Repetitions; i++) { hand.Trajectory.Add(hand.Target.Transform.Clone()); hand.Trajectory.Add(hand.Subject.Transform.Clone()); } } } return(new MBoolResponse(true)); }
public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState) { //Extract the hand information if (instruction.Properties.ContainsKey("Hand")) { switch (instruction.Properties["Hand"]) { case "Left": this.SetupHand(MJointType.LeftWrist, instruction); break; case "Right": this.SetupHand(MJointType.RightWrist, instruction); break; case "Both": this.SetupHand(MJointType.LeftWrist, instruction); this.SetupHand(MJointType.RightWrist, instruction); break; } } else { this.SetupHand(MJointType.RightWrist, instruction); } //To fix -> might overwrite already active hands foreach (HandContainer hand in this.activeHands) { if (hand.Trajectory == null && !hand.Instruction.Properties.ContainsKey("TargetID") && !hand.Instruction.Properties.ContainsKey("targetID")) { MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, "At least one of both trajectory or target must be set. Aborting Move MMU"); return(new MBoolResponse(false)); } //Get the global hand position and rotation MVector3 handPosition = GetGlobalPosition(simulationState.Initial, hand.Type); MQuaternion handRotation = GetGlobalRotation(simulationState.Initial, hand.Type); //Get the transform of the scene object to be moved MTransform sceneObjectTransform = this.SceneAccess.GetTransformByID(hand.Instruction.Properties.GetValue("SubjectID", "subjectID")); //Compute the relative transform of the hand hand.Offset = new MTransform("", sceneObjectTransform.InverseTransformPoint(handPosition), sceneObjectTransform.InverseTransformRotation(handRotation)); //bool addPseudoPoint = true; //if(hand.Trajectory == null && addPseudoPoint) //{ // //Compute artifical trajectory // hand.Trajectory = new List<MTransform>() { sceneObjectTransform.Clone(), this.ComputeTargetTransform(hand) }; // //Insert intermediate point that prevents from intersection //} //Set hand to initialized hand.Initialized = true; } return(new MBoolResponse(true)); }
public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState) { //Assign the instruction this.instruction = instruction; //Reset the flags and states this.positioningFinished = false; this.rootPositionLastFrame = null; // Initialize IK Service this.ServiceAccess.IKService.Setup(this.AvatarDescription, new Dictionary <string, string>()); //Get the target id if (instruction.Properties.ContainsKey("TargetID")) { this.objectTransform = this.SceneAccess.GetTransformByID(instruction.Properties["TargetID"]); } //Error id not available else { return(new MBoolResponse(false) { LogData = new List <string>() { "Required parameter TargetID not defined" } }); } //Get the target hand if (instruction.Properties.ContainsKey("Hand")) { if (instruction.Properties["Hand"] == "Left") { this.handJoint = MJointType.LeftWrist; } if (instruction.Properties["Hand"] == "Right") { this.handJoint = MJointType.RightWrist; } } //Error target hand not specified else { return(new MBoolResponse(false) { LogData = new List <string>() { "Required parameter hand not defined" } }); } //Parse optional property if (instruction.Properties.ContainsKey("PositioningFinishedThreshold")) { float.TryParse(instruction.Properties["PositioningFinishedThreshold"], out this.positioningFinishedThreshold); } //Parse optional property if (instruction.Properties.ContainsKey("AddOffset")) { bool.TryParse(instruction.Properties["AddOffset"], out addOffset); } //Extract the velocity if defined if (instruction.Properties.ContainsKey("Velocity")) { float.TryParse(instruction.Properties["Velocity"], out velocity); } //Get the initial position and rotation this.SkeletonAccess.SetChannelData(simulationState.Initial); MVector3 currentHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint); MQuaternion currentHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint); //Create a new transform representing the "virtual" transform of the hand MTransform handTransform = new MTransform("hand", currentHandPosition, currentHandRotation); //Compute the offsets between hand <-> object this.objectPositionOffset = handTransform.InverseTransformPoint(objectTransform.Position); this.objectRotationOffset = handTransform.InverseTransformRotation(objectTransform.Rotation); return(new MBoolResponse(true)); }