/// <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 DoStepBlending(double time, MSimulationState simulationState)
{
//Create a new simulation result
MSimulationResult result = new MSimulationResult()
{
Events = simulationState.Events ?? new List <MSimulationEvent>(),
Constraints = simulationState.Constraints,
SceneManipulations = simulationState.SceneManipulations ?? new List <MSceneManipulation>(),
Posture = simulationState.Current
};
//Directly operate on the global constraints -> since no ik is computed
List <MConstraint> globalConstraints = result.Constraints;
//Assign the global constraints to the constraint manager
this.constraintManager.SetConstraints(ref globalConstraints);
//Use the initial state (approved posture of last frame)
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);
MJointConstraint jointConstraint = null;
switch (this.handJoint)
{
case MJointType.LeftWrist:
jointConstraint = this.constraintManager.GetEndeffectorConstraint(MJointType.LeftWrist);
break;
case MJointType.RightWrist:
jointConstraint = this.constraintManager.GetEndeffectorConstraint(MJointType.RightWrist);
break;
}
//Handle the joint constraint
if (jointConstraint != null)
{
//Get the current hand positon based on the constraint
currentHandPosition = jointConstraint.GeometryConstraint.GetGlobalPosition(this.SceneAccess);
currentHandRotation = jointConstraint.GeometryConstraint.GetGlobalRotation(this.SceneAccess);
}
//Set the skeleton to the current state
this.SkeletonAccess.SetChannelData(simulationState.Current);
//Determine the target hand position (either underlying MMU or given via boundary constraints)
MVector3 targetHandPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, this.handJoint);
MQuaternion targetHandRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, this.handJoint);
//Move the hand from the current position to the target position
MVector3 deltaPosition = targetHandPosition.Subtract(currentHandPosition);
float angle = (float)MQuaternionExtensions.Angle(currentHandRotation, targetHandRotation);
//Compute the distance of the hand to the target hand position
float distanceToGoal = deltaPosition.Magnitude();
//Compute the max distance which can be covered within the current frame
float maxDistance = (float)(time * this.velocity);
//Compute the max allowed angle
float maxAngle = (float)(time * this.angularVelocity);
//Compute the weight for slerping (weight increases with shrinking distance to target)
float translationWeight = Math.Min(1.0f, maxDistance / distanceToGoal);
//Compute the rotation weight
float rotationWeight = Math.Min(1.0f, maxAngle / angle);
//Blend from the current rotation to the target
result.Posture = Blending.PerformBlend((IntermediateSkeleton)this.SkeletonAccess, simulationState.Initial, simulationState.Current, Math.Min(translationWeight, rotationWeight), true);
this.SkeletonAccess.SetChannelData(result.Posture);
if (distanceToGoal < 0.01f)
{
result.Events.Add(new MSimulationEvent()
{
Name = "Release Finished",
Reference = this.instruction.ID,
Type = mmiConstants.MSimulationEvent_End
});
//Remove all constraints for the respective hand
this.constraintManager.RemoveEndeffectorConstraints(this.handJoint);
}
else
{
//Remove the endeffector constraint
this.constraintManager.RemoveEndeffectorConstraints(this.handJoint);
//Update the constraint
this.constraintManager.SetEndeffectorConstraint(this.handJoint, this.SkeletonAccess.GetGlobalJointPosition(result.Posture.AvatarID, this.handJoint), this.SkeletonAccess.GetGlobalJointRotation(result.Posture.AvatarID, this.handJoint));
}
return(result);
}