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