/// <summary>
/// Performs the actual solving
/// </summary>
/// <param name="input"></param>
/// <param name="mmuResults"></param>
/// <returns></returns>
public virtual MSimulationResult Solve(MSimulationResult input, List <MSimulationResult> mmuResults, float timespan)
{
MSimulationResult result = input;
//By default use all constraints
List <MConstraint> constraints = new List <MConstraint>(input.Constraints);
//Only solve the constraints which are not already fulfilled
if (this.SolveViolatedConstraintsOnly)
{
constraints = this.GetViolatedIKConstraints(input.Constraints, result.Posture);
}
//Compute the ik if at least one constraint is not fulfilled
if (constraints.Count > 0)
{
MAvatarPostureValues currentPosture = input.Posture;
//Solve n times
for (int i = 0; i < this.Iterations; i++)
{
MIKServiceResult ikResult = this.serviceAccess.IKService.CalculateIKPosture(currentPosture, constraints, new Dictionary <string, string>());
currentPosture = ikResult.Posture;
}
result.Posture = currentPosture;
}
return(result);
}
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>()
};
List <MConstraint> constraints = new List <MConstraint>();
//Apply ik
if (LeftHandTarget != null)
{
constraints.Add(new MConstraint(System.Guid.NewGuid().ToString())
{
JointConstraint = new MJointConstraint()
{
GeometryConstraint = new MGeometryConstraint("")
{
ParentToConstraint = new MTransform(System.Guid.NewGuid().ToString(), LeftHandTarget.Transform.Position, LeftHandTarget.Transform.Rotation),
WeightingFactor = 1.0f,
},
JointType = MJointType.LeftWrist
}
});
}
if (RightHandTarget != null)
{
constraints.Add(new MConstraint(System.Guid.NewGuid().ToString())
{
JointConstraint = new MJointConstraint()
{
GeometryConstraint = new MGeometryConstraint("")
{
ParentToConstraint = new MTransform(System.Guid.NewGuid().ToString(), RightHandTarget.Transform.Position, RightHandTarget.Transform.Rotation),
WeightingFactor = 1.0f
},
JointType = MJointType.RightWrist
},
});
}
if (constraints.Count > 0)
{
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(simulationState.Current, constraints, new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
return(result);
}
public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState)
{
//Setup the ik
this.ServiceAccess.IKService.Setup(this.AvatarDescription, new Dictionary <string, string>());
//Reset all flags/states
this.constraintManager = new ConstraintManager(this.SceneAccess);
this.singleShotIK = false;
this.singleShotIKTargetPosture = null;
//Assign the instruction
this.instruction = instruction;
//Parse the parameters
MBoolResponse result = this.ParseParameters(instruction);
if (!result.Successful)
{
return(result);
}
//Compute the target posture
if (this.singleShotIK)
{
List <MConstraint> tempConstraints = new List <MConstraint>();
constraintManager.SetConstraints(ref tempConstraints);
//Set the ik constraints
constraintManager.SetEndeffectorConstraint(this.handJoint, targetTransform.Position, targetTransform.Rotation);
//Compute the posture
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(simulationState.Current, constraintManager.GetJointConstraints(), new Dictionary <string, string>());
this.singleShotIKTargetPosture = ikResult.Posture.Copy();
//Clear the constraints in the constraint manager
tempConstraints.Clear();
}
//Return true/success
return(new MBoolResponse(true));
}
/// <summary>
/// Performs the do step realted computations for both handed mode
/// </summary>
/// <param name="result"></param>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
private MSimulationResult DoStepBothHanded(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);
switch (this.bothHandedState)
{
//First position the object in order to carry it properly
case CarryState.Positioning:
//Perform both handed positioning
this.PositionObjectBothHanded(ref result, time);
//Solve using ik if constraints are defined
if (this.constraintManager.GetJointConstraints().Count > 0)
{
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, this.constraintManager.GetJointConstraints(), new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
break;
case CarryState.Carry:
//Perform a both handed carry (just preserve the relative position and rotation)
this.BothHandedCarry(ref result);
//Solve using ik if constraints are defined
if (this.constraintManager.GetJointConstraints().Count > 0)
{
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, this.constraintManager.GetJointConstraints(), new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
break;
}
return(result);
}
/// <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>
/// 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;
}
}
}
/// <summary>
/// Do step routine in which the actual simulation result is generated
/// </summary>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new simulation result
MSimulationResult result = new MSimulationResult()
{
Events = new List <MSimulationEvent>(),
Constraints = simulationState.Constraints ?? new List <MConstraint>(),
SceneManipulations = new List <MSceneManipulation>(),
Posture = simulationState.Current
};
//Compute the target transform at the beginning of each frame
this.targetTransform = this.ComputeTargetTransform();
//The presently active constraints
List <MConstraint> globalConstraints = result.Constraints;
//The local constraints defined within the MMU
List <MConstraint> localConstraints = new List <MConstraint>();
//Use the constraint manager to manage the local constraints
constraintManager.SetConstraints(ref localConstraints);
//Set the channel data to the approved state of the last frame (all MMUs were executed including the low prio grasp/positioning)
this.SkeletonAccess.SetChannelData(simulationState.Initial);
//Get the current hand position and rotation
MVector3 currentHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint);
MQuaternion currentHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint);
//The next pose
MTransform nextPose = null;
//The current velocity used for path planning
float currentVelocity = this.velocity;// + this.ComputeRootVelocity(time, simulationState);
//Use the trajectory if defined
if (this.trajectory != null)
{
//If a trajectory is used -> The target transform is the last point of the trajectory
this.targetTransform = this.trajectory.Last();
//Compute the next pose
nextPose = this.DoLocalMotionPlanning(currentVelocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, this.trajectory[trajectoryIndex].Position, this.trajectory[trajectoryIndex].Rotation);
//Check if close to current target -> move to next target -> To do consider rotation
if ((nextPose.Position.Subtract(trajectory[trajectoryIndex].Position)).Magnitude() < this.translationThreshold && MQuaternionExtensions.Angle(nextPose.Rotation, trajectory[trajectoryIndex].Rotation) < this.rotationThreshold && trajectoryIndex < trajectory.Count - 1)
{
trajectoryIndex++;
}
}
else
{
//Compute the next pose
nextPose = this.DoLocalMotionPlanning(currentVelocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, this.targetTransform.Position, this.targetTransform.Rotation);
}
//Get the current distance
float currentDistance = (nextPose.Position.Subtract(targetTransform.Position)).Magnitude();
float currentAngularDistance = (float)MQuaternionExtensions.Angle(nextPose.Rotation, targetTransform.Rotation);
//Check if the ik is only computed once and blending is performed subsequently
if (this.singleShotIK)
{
//Estimate the weight for blending
float weight = (float)Math.Min(1, (currentVelocity * time) / currentDistance);
//To check -> Why is a deep copy required?
result.Posture = Blending.PerformBlend((IntermediateSkeleton)this.SkeletonAccess, simulationState.Initial, this.singleShotIKTargetPosture.Copy(), weight, false);
if (weight >= 1 - 1e-3)
{
result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
constraintManager.SetEndeffectorConstraint(new MJointConstraint(this.handJoint)
{
GeometryConstraint = new MGeometryConstraint()
{
ParentObjectID = "",
ParentToConstraint = new MTransform(System.Guid.NewGuid().ToString(), targetTransform.Position, targetTransform.Rotation)
}
});
}
}
//Default scenario -> IK is computed for each frame
else
{
if (currentDistance <= this.translationThreshold && currentAngularDistance <= this.rotationThreshold)
{
//Set the target
nextPose.Position = targetTransform.Position;
nextPose.Rotation = targetTransform.Rotation;
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, "Reach finished");
result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
//Set the desired endeffector constraints
constraintManager.SetEndeffectorConstraint(this.handJoint, nextPose.Position, nextPose.Rotation);
}
//Create a list with the specific constraints for the reach MMU -> Only get the specific ones that must be solved (local constraints)
List <MConstraint> ikConstraints = constraintManager.GetJointConstraints();
//Only solve if at least one constraint is defined
if (ikConstraints.Count > 0)
{
int ikIterations = 1;
MIKServiceResult ikResult = null;
//Use the ik to compute a posture fulfilling the requested constraints
//To do -> Try with different initial postures / compute suitability of the generated posture
for (int i = 0; i < ikIterations; i++)
{
//Compute twice
ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, ikConstraints, new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
}
//Update the constraint manager to operate on the global constraints
constraintManager.SetConstraints(ref globalConstraints);
//Integrate the newly defined constraints in the global ones
constraintManager.Combine(localConstraints);
//Just for better understanding -> Assign the previous constraints + integrated ones to the result (this is not neccessary since the constraint manager is operating on the reference)
result.Constraints = globalConstraints;
//Return the result
return(result);
}
/// <summary>
/// Basic do step routine that is executed for each frame and generates the actual motion.
/// </summary>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
private MSimulationResult DoStepIK(double time, MSimulationState simulationState)
{
//Create a default result
MSimulationResult result = new MSimulationResult()
{
Events = new List <MSimulationEvent>(),
Constraints = simulationState.Constraints ?? new List <MConstraint>(),
SceneManipulations = new List <MSceneManipulation>(),
Posture = simulationState.Current
};
//The presently active constraints
List <MConstraint> globalConstraints = result.Constraints;
//The local constraints defined within the MMU
List <MConstraint> localConstraints = new List <MConstraint>();
//Use the constraint manager to manage the local constraints
constraintManager.SetConstraints(ref localConstraints);
//Set the channel data to the approved state of the last frame (all MMUs were executed including the low prio grasp/positioning)
this.SkeletonAccess.SetChannelData(simulationState.Initial);
//Get the current hand position and rotation
MVector3 currentHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint);
MQuaternion currentHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint);
//Set the skeleton acess data to the current
this.SkeletonAccess.SetChannelData(simulationState.Current);
//Determine the target hand position (either underlying MMU or given via boundary constraints)
MTransform targetTransform = new MTransform()
{
Position = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint),
Rotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint)
};
MTransform nextPose = null;
float translationWeight = 0;
//Use the trajectory if defined
if (this.trajectory != null && this.trajectory.Count > 0)
{
//Update the last element dynamically
trajectory.Last().Position = targetTransform.Position;
trajectory.Last().Rotation = targetTransform.Rotation;
//Compute the next pose
nextPose = this.DoLocalMotionPlanning(this.velocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, this.trajectory[trajectoryIndex].Position, this.trajectory[trajectoryIndex].Rotation, out translationWeight);
//Check if close to current target -> move to next target -> To do consider rotation
if ((nextPose.Position.Subtract(trajectory[trajectoryIndex].Position)).Magnitude() < 0.1f && MQuaternionExtensions.Angle(nextPose.Rotation, trajectory[trajectoryIndex].Rotation) < 1f && trajectoryIndex < trajectory.Count - 1)
{
trajectoryIndex++;
}
}
else
{
//Compute the next pose
nextPose = this.DoLocalMotionPlanning(this.velocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, targetTransform.Position, targetTransform.Rotation, out translationWeight);
}
////Determine the next pose using local motion planning
//MTransform nextPose = this.DoLocalMotionPlanning(this.velocity, this.angularVelocity, TimeSpan.FromSeconds(time), currentHandPosition, currentHandRotation, targetHandPosition, targetHandRotation, out float translationWeight);
//Perform a partial blend
//result.Posture = this.PerformPartialBlend(this.handJoint, translationWeight, simulationState);
//Get the current distance
float currentDistance = (nextPose.Position.Subtract(targetTransform.Position)).Magnitude();
float currentAngularDistance = (float)MQuaternionExtensions.Angle(nextPose.Rotation, targetTransform.Rotation);
//Handle the present state (either in ik mode oder blending)
switch (this.state)
{
case ReleaseMotionState.IK:
if (currentDistance < 0.02f && currentAngularDistance < 5f)
{
//Switch to blending to realize the final share
this.state = ReleaseMotionState.Blending;
this.elapsedBlendTime = 0;
//Set to global constraints
this.constraintManager.SetConstraints(ref globalConstraints);
//Remove all constraints for the respective hand
this.constraintManager.RemoveEndeffectorConstraints(this.handJoint);
}
else
{
//Update the constraint
this.constraintManager.SetEndeffectorConstraint(this.handJoint, nextPose.Position, nextPose.Rotation);
}
break;
}
//Use the local constraint to compute the ik
this.constraintManager.SetConstraints(ref localConstraints);
//React depending on the given state
switch (this.state)
{
//In ik mode the ik solver must be called
case ReleaseMotionState.IK:
//Create a list with the specific constraints for the reach MMU -> Only get the specific ones that must be solved (local constraints)
List <MConstraint> ikConstraints = constraintManager.GetJointConstraints();
//Only solve if at least one constraint is defined
if (ikConstraints.Count > 0)
{
//Compute twice
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(result.Posture, ikConstraints, new Dictionary <string, string>());
result.Posture = ikResult.Posture;
}
break;
//In blending mode, motion blending must be performed
case ReleaseMotionState.Blending:
//Perform a blend
elapsedBlendTime += (float)time;
float blendWeight = Math.Min(1, elapsedBlendTime / endBlendDuration);
result.Posture = MMICSharp.Common.Tools.Blending.PerformBlend(this.SkeletonAccess as IntermediateSkeleton, simulationState.Initial, simulationState.Current, blendWeight, true);
//Perform a partial blend
//result.Posture = this.PerformPartialBlend(this.handJoint, blendWeight, simulationState);
if (blendWeight >= 1f)
{
result.Events.Add(new MSimulationEvent()
{
Name = "Release Finished",
Reference = this.instruction.ID,
Type = mmiConstants.MSimulationEvent_End
});
}
break;
}
//Combine the constraints
this.constraintManager.SetConstraints(ref globalConstraints);
this.constraintManager.Combine(localConstraints);
return(result);
}
/// <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);
}
/// <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);
}
// Update is called once per frame
void LateUpdate()
{
if (EnableIK)
{
MMIAvatar avatar = this.GetComponent <MMIAvatar>();
if (NewInterface)
{
List <MConstraint> constraints = new List <MConstraint>();
if (LeftHandTarget != null)
{
MJointConstraint leftHandConstraint = new MJointConstraint(MJointType.LeftWrist)
{
GeometryConstraint = new MGeometryConstraint()
{
ParentObjectID = "",
ParentToConstraint = this.LeftHandTarget.MSceneObject.Transform
}
};
constraints.Add(new MConstraint(System.Guid.NewGuid().ToString())
{
JointConstraint = leftHandConstraint
});
}
if (RightHandTarget != null)
{
MJointConstraint rightHandConstraint = new MJointConstraint(MJointType.RightWrist)
{
GeometryConstraint = new MGeometryConstraint()
{
ParentObjectID = "",
ParentToConstraint = this.RightHandTarget.MSceneObject.Transform
}
};
constraints.Add(new MConstraint(System.Guid.NewGuid().ToString())
{
JointConstraint = rightHandConstraint
});
}
MIKServiceResult result = avatar.MMUAccess.ServiceAccess.IKService.CalculateIKPosture(avatar.GetRetargetedPosture(), constraints, new Dictionary <string, string>());
avatar.AssignPostureValues(result.Posture);
}
else
{
List <MIKProperty> ikProperties = new List <MIKProperty>();
if (LeftHandTarget != null)
{
ikProperties.Add(new MIKProperty()
{
OperationType = MIKOperationType.SetPosition,
Target = MEndeffectorType.LeftHand,
Values = new List <double>()
{
LeftHandTarget.transform.position.x, LeftHandTarget.transform.position.y, LeftHandTarget.transform.position.z
},
Weight = 1
});
ikProperties.Add(new MIKProperty()
{
OperationType = MIKOperationType.SetRotation,
Target = MEndeffectorType.LeftHand,
Values = new List <double>()
{
LeftHandTarget.transform.rotation.x, LeftHandTarget.transform.rotation.y, LeftHandTarget.transform.rotation.z, LeftHandTarget.transform.rotation.w
},
Weight = 1
});
}
if (RightHandTarget != null)
{
ikProperties.Add(new MIKProperty()
{
OperationType = MIKOperationType.SetPosition,
Target = MEndeffectorType.RightHand,
Values = new List <double>()
{
RightHandTarget.transform.position.x, RightHandTarget.transform.position.y, RightHandTarget.transform.position.z
},
Weight = 1
});
ikProperties.Add(new MIKProperty()
{
OperationType = MIKOperationType.SetRotation,
Target = MEndeffectorType.RightHand,
Values = new List <double>()
{
RightHandTarget.transform.rotation.x, RightHandTarget.transform.rotation.y, RightHandTarget.transform.rotation.z, RightHandTarget.transform.rotation.w
},
Weight = 1
});
}
MAvatarPostureValues posture = avatar.MMUAccess.ServiceAccess.IKService.ComputeIK(avatar.GetRetargetedPosture(), ikProperties);
avatar.AssignPostureValues(posture);
}
}
}
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new simulation result
MSimulationResult result = new MSimulationResult()
{
Events = simulationState.Events ?? new List <MSimulationEvent>(),
Constraints = simulationState.Constraints ?? new List <MConstraint>(),
SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>()
};
//Assign the constraints to a temp varilable
List <MConstraint> constraints = result.Constraints;
//Use the constraint manager to manage the constraints
constraintManager.SetConstraints(ref constraints);
//Get the hand position and rotation of the last frame (approved result)
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);
//Get the desired hand position (of the underlying motion e.g. idle)
this.SkeletonAccess.SetChannelData(simulationState.Current);
MVector3 targetHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint);
MQuaternion targetHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint);
//Add an offset on top of the position if desired
if (this.addOffset)
{
targetHandPosition = ComputeNewPositionWithOffset(targetHandPosition, simulationState.Current);
}
//Move the hand from the current position to the target position
MVector3 deltaPosition = targetHandPosition.Subtract(currentHandPosition);
//Compute the distance of the hand to the target hand position
float distanceToGoal = deltaPosition.Magnitude();
//Create positioning finished event if not already created and distance below threshold
if (distanceToGoal < this.positioningFinishedThreshold && !this.positioningFinished)
{
result.Events.Add(new MSimulationEvent("PositioningFinished", "PositioningFinished", this.instruction.ID));
this.positioningFinished = true;
}
//Compute the current velocity based on the general max velocity and the velocity of the root motion
float currentVelocity = this.velocity + this.ComputeRootVelocity(time, simulationState);
//Compute the max distance which can be covered within the current frame
float maxDistance = (float)(time * currentVelocity);
//Compute the weight for slerping (weight increases with shrinking distance to target)
float weight = Math.Max(0, 1 - distanceToGoal);
//Create a new transform representing the next hand transform
MTransform newHandTransform = new MTransform("", currentHandPosition.Clone(), currentHandRotation.Clone())
{
//Compute the new hand position (normalize delta position and multiply by max distance)
Position = currentHandPosition.Add(deltaPosition.Normalize().Multiply(Math.Min(deltaPosition.Magnitude(), maxDistance))),
//Just perform an interpolation to gather new hand rotation (weight is determined by the translation distance)
Rotation = MQuaternionExtensions.Slerp(currentHandRotation, targetHandRotation, weight)
};
//Compute the corresponding positon/rotation of the object and
//adjust the transformation of the object which should be moved
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = this.objectTransform.ID,
//Compute the new global position of the object
Position = newHandTransform.TransformPoint(this.objectPositionOffset),
//Compute the new global rotation of the object
Rotation = newHandTransform.TransformRotation(this.objectRotationOffset)
}
}
});
//Set the desired endeffector constraints
constraintManager.SetEndeffectorConstraint(this.handJoint, newHandTransform.Position, newHandTransform.Rotation);
//Generate a new posture using the ik solver and the specified constraints
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(simulationState.Current, constraintManager.GetJointConstraints(), new Dictionary <string, string>());
result.Posture = ikResult.Posture;
//Return the result
return(result);
}
/// <summary>
/// Do step routine in which the actual simulation result is generated
/// </summary>
/// <param name="time"></param>
/// <param name="simulationState"></param>
/// <returns></returns>
public override MSimulationResult DoStep(double time, MSimulationState simulationState)
{
//Create a new simulation result
MSimulationResult result = new MSimulationResult()
{
Events = simulationState.Events != null ? simulationState.Events : new List <MSimulationEvent>(),
Constraints = simulationState.Constraints,
SceneManipulations = simulationState.SceneManipulations != null ? simulationState.SceneManipulations : new List <MSceneManipulation>()
};
//Create variables representing the next object position/rotation
MTransform nextObjectTransform = subjectTransform.Clone();
//Use the constraint manager to manage the constraints
List <MConstraint> tmpConstraints = result.Constraints;
//Set the constraints
constraintManager.SetConstraints(ref tmpConstraints);
//Compute the new hand position and rotation
MVector3 deltaPosition = this.targetObjectTransform.Position.Subtract(subjectTransform.Position);
float distanceToGoal = deltaPosition.Magnitude();
//Get the current object position
float maxDistance = (float)time * 1.0f;
//Check the current distance to goal
if (distanceToGoal < 0.01f)
{
result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
else
{
//Compute the new hand position (normalize delta position and multiply by max distance)
nextObjectTransform.Position = this.subjectTransform.Position.Add(deltaPosition.Normalize().Multiply(Math.Min(distanceToGoal, maxDistance)));
//Compute the weight for slerping (weight increases with shrinking distance to target)
float weight = Math.Max(0, 1 - distanceToGoal);
//Just perform an interpolation to gather new hand rotation (weight is determined by the translation distance)
nextObjectTransform.Rotation = MQuaternionExtensions.Slerp(this.subjectTransform.Rotation, this.targetObjectTransform.Rotation, weight);
}
//Adjust the transformation of the object which should be moved
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = this.subjectTransform.ID,
Position = nextObjectTransform.Position,
Rotation = nextObjectTransform.Rotation
}
}
});
//Get the current hand position in global space
MVector3 globalHandPosition = nextObjectTransform.TransformPoint(this.handPositionOffset);
MQuaternion globalHandRotation = nextObjectTransform.TransformRotation(this.handRotationOffset);
//Set the desired endeffector constraints
constraintManager.SetEndeffectorConstraint(this.handJoint, globalHandPosition, globalHandRotation);
MIKServiceResult ikResult = this.ServiceAccess.IKService.CalculateIKPosture(simulationState.Current, constraintManager.GetJointConstraints(), new Dictionary <string, string>());
//Generate a new posture using the ik solver and the specified constraints
result.Posture = ikResult.Posture;
//Return the result
return(result);
}
