/// <summary>
/// Performs the actual motion planning
/// </summary>
/// <param name="velocity"></param>
/// <param name="angularVelocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(float velocity, float angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
MTransform result = new MTransform();
MVector3 delta = targetPosition.Subtract(currentPosition);
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
double maxTranslationDelta = velocity * time.TotalSeconds;
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
//To do consider self collision
double maxAngle = angularVelocity * time.TotalSeconds;
if (angle < maxAngle)
{
angle = maxAngle;
}
double weight = Math.Min(1, maxAngle / angle);
result.Position = currentPosition.Add(delta);
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, (float)weight);
//result.Time = time;
return(result);
}
/// <summary>
/// Returns a list of MTRansform
/// </summary>
/// <param name="pathConstraint"></param>
/// <returns></returns>
public static List <MTransform> GetMTransformList(this MPathConstraint pathConstraint)
{
//Create a list for storing the full trajectory
List <MTransform> list = new List <MTransform>();
foreach (MGeometryConstraint mg in pathConstraint.PolygonPoints)
{
MTransform t = null;
if (mg.ParentToConstraint != null)
{
t = new MTransform
{
ID = "",
Position = mg.ParentToConstraint.Position,
Rotation = mg.ParentToConstraint.Rotation
};
}
else
{
t = new MTransform
{
ID = "",
Position = mg.TranslationConstraint.GetVector3(),
Rotation = MQuaternionExtensions.FromEuler(mg.TranslationConstraint.GetVector3())
};
}
list.Add(t);
}
return(list);
}
/// <summary>
/// Performs a local motion planning to estimate the next pose
/// </summary>
/// <param name="velocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
//Create a resulting transform
MTransform result = new MTransform();
//Estimate the delta
MVector3 deltaPosition = targetPosition.Subtract(currentPosition);
//Estimate the meximum allowed delta
double maxTranslationDelta = velocity * time.TotalSeconds;
//Limit the maximum
if (deltaPosition.Magnitude() >= maxTranslationDelta)
{
deltaPosition = deltaPosition.Normalize();
deltaPosition = deltaPosition.Multiply(maxTranslationDelta);
}
float angularVelocityReach = 100f;
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
double maxAngle = angularVelocityReach * time.TotalSeconds;
//Estimate the blendweihgt for the oreitnation blending
double weight = Math.Min(1, maxAngle / angle);
result.Position = currentPosition.Add(deltaPosition);
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, (float)weight);
//result.Time = time;
return(result);
}
/// <summary>
/// Method is repsonsible for modeling the actual carry for both handed objects
/// </summary>
/// <param name="result"></param>
private void BothHandedCarry(ref MSimulationResult result)
{
//Create an empty transform representing the next object transform
MTransform nextObjectTransform = new MTransform();
//Update the desired object transform
if (this.CarryTargetName != null && this.CarryTargetName.Length > 0)
{
MTransform targetTransform = SceneAccess.GetTransformByID(this.CarryTargetName);
nextObjectTransform.Position = targetTransform.Position;
nextObjectTransform.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));
nextObjectTransform.Position = refTransform.TransformPoint(this.internalCarryTransform.Position);
nextObjectTransform.Rotation = refTransform.TransformRotation(this.internalCarryTransform.Rotation);
}
//Compute the object transform
result.SceneManipulations.Add(new MSceneManipulation()
{
Transforms = new List <MTransformManipulation>()
{
new MTransformManipulation()
{
Target = instruction.Properties["TargetID"],
Position = nextObjectTransform.Position,
Rotation = nextObjectTransform.Rotation
}
}
});
List <MIKProperty> ikProperties = new List <MIKProperty>();
//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));
this.constraintManager.SetEndeffectorConstraint(hand.JointType, nextHandPose.Position, nextHandPose.Rotation, hand.ConstraintID);
}
}
/// <summary>
/// Performs local motion planning to reach the defined point
/// </summary>
/// <param name="velocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, double angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation)
{
//Create a new transform representing the result
MTransform result = new MTransform();
//Estimate the vector to reach the goal
MVector3 delta = targetPosition.Subtract(currentPosition);
float distance = delta.Magnitude();
//Determine the angular distance
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
//Determine the max translation delta and max angle
double maxTranslationDelta = velocity * time.TotalSeconds;
double maxAngle = angularVelocity * time.TotalSeconds;
//Compute the translation weight
float translationWeight = (float)Math.Min(1, maxTranslationDelta / delta.Magnitude());
//Compute the rotation weight
float rotationWeight = (float)Math.Min(1, maxAngle / angle);
//Limit the translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
//Compute the new position
result.Position = currentPosition.Add(delta);
if (angularVelocity == 0)
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, translationWeight);
}
else
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, rotationWeight);
}
return(result);
}
private void SetBaseReference(MQuaternion baseGlobalRot, MVector3 baseGlobalPos)
{
this.isMapped = true;
this.targetBP = baseGlobalPos.Clone();
this.targetGBR = baseGlobalRot.Clone();
this.isBP = this.GetGlobalPosition();
this.isGBR = this.GetGlobalRotation();
this.invTargetGBR = MQuaternionExtensions.Inverse(this.targetGBR);
this.invIsGBR = MQuaternionExtensions.Inverse(this.isGBR);
this.isOffset = this.invIsGBR.Multiply(this.targetBP.Subtract(this.isBP));
this.targetOffset = this.invTargetGBR.Multiply(this.isBP.Subtract(this.targetBP));
this.inverseOffsetRotation = MQuaternionExtensions.Inverse(this.GetOffsetRotation());
}
/// <summary>
/// Determines whether the constraint is fulflled
/// </summary>
/// <param name="desiredRotation"></param>
/// <param name="currentRotation"></param>
/// <param name="rotationConstraint"></param>
/// <returns></returns>
private bool IsFulfilled(MQuaternion desiredRotation, MQuaternion currentRotation, MRotationConstraint rotationConstraint)
{
//By default return true -> It is assumed that interval is [-inv,+inv]
if (rotationConstraint == null)
{
return(true);
}
MVector3 currentRotationEuler = MQuaternionExtensions.ToEuler(currentRotation);
//Determine the global min and max coordinate
MVector3 min = new MVector3(rotationConstraint.Limits.X.Min, rotationConstraint.Limits.Y.Min, rotationConstraint.Limits.Z.Min).Add(currentRotationEuler);
MVector3 max = new MVector3(rotationConstraint.Limits.X.Max, rotationConstraint.Limits.Y.Max, rotationConstraint.Limits.Z.Max).Add(currentRotationEuler);
return(currentRotationEuler.X >= min.X && currentRotationEuler.Y >= min.Y && currentRotationEuler.Z >= min.Z && currentRotationEuler.X <= max.X && currentRotationEuler.Y <= max.Y && currentRotationEuler.Z <= max.Z);
}
private MAvatarPostureValues PerformPartialBlend(MJointType bodySide, float weight, MSimulationState simulationState)
{
List <MJointType> toConsider = new List <MJointType>();
switch (bodySide)
{
case MJointType.LeftWrist:
toConsider.Add(MJointType.LeftShoulder);
toConsider.Add(MJointType.LeftElbow);
toConsider.Add(MJointType.LeftWrist);
break;
case MJointType.RightWrist:
toConsider.Add(MJointType.RightShoulder);
toConsider.Add(MJointType.RightElbow);
toConsider.Add(MJointType.RightWrist);
break;
}
Dictionary <MJointType, MQuaternion> rotations = new Dictionary <MJointType, MQuaternion>();
foreach (MJointType jt in toConsider)
{
this.SkeletonAccess.SetChannelData(simulationState.Initial);
MQuaternion rot1 = this.SkeletonAccess.GetLocalJointRotation(this.AvatarDescription.AvatarID, jt);
this.SkeletonAccess.SetChannelData(simulationState.Current);
MQuaternion rot2 = this.SkeletonAccess.GetLocalJointRotation(this.AvatarDescription.AvatarID, jt);
MQuaternion interpolatedRotation = MQuaternionExtensions.Slerp(rot1, rot2, weight);
rotations.Add(jt, interpolatedRotation);
}
this.SkeletonAccess.SetChannelData(simulationState.Current);
foreach (var entry in rotations)
{
this.SkeletonAccess.SetLocalJointRotation(this.AvatarDescription.AvatarID, entry.Key, entry.Value);
}
return(this.SkeletonAccess.RecomputeCurrentPostureValues(this.AvatarDescription.AvatarID));
}
public void RecomputeLocalTransformations()
{
MQuaternion parentRotation = new MQuaternion(0, 0, 0, 1);
MVector3 parentPosition = new MVector3(0, 0, 0);
if (this.parentJoint != null)
{
parentRotation = this.parentJoint.GetGlobalRotation();
parentPosition = this.parentJoint.GetGlobalPosition();
}
MQuaternion inverseParentRot = MQuaternionExtensions.Inverse(parentRotation);
if (!isMapped && this.parentJoint == null)
{
this.currentRotationValues = new MQuaternion(0, 0, 0, 1);
this.translation = this.globalPos;
}
else if (isMapped)
{
this.currentRotationValues = this.inverseOffsetRotation.Multiply(inverseParentRot).Multiply(this.globalRot);
MVector3 rotatedOffset = parentRotation.Multiply(this.joint.Position);
MVector3 rotatedTranslation = this.globalPos.Subtract(parentPosition).Subtract(rotatedOffset);
this.translation = this.inverseOffsetRotation.Multiply(inverseParentRot).Multiply(rotatedTranslation);
}
else
{
this.currentRotationValues = new MQuaternion(0, 0, 0, 1);
this.translation = new MVector3(0, 0, 0);
this.globalPos = null;
this.globalRot = null;
this.GetGlobalPosition();
this.GetGlobalRotation();
}
foreach (Joint c in this.children)
{
((RJoint)c).RecomputeLocalTransformations();
}
}
/// <summary>
/// Rotates the current transform around the specific point and axis
/// </summary>
/// <param name="center">The rotation center</param>
/// <param name="axis">The rotation axis</param>
/// <param name="angle">The angle to rotate</param>
private static MTransform RotateAround(MTransform transform, MVector3 center, MVector3 axis, float angle)
{
MTransform res = new MTransform()
{
ID = System.Guid.NewGuid().ToString()
};
MVector3 pos = transform.Position;
MQuaternion rot = MQuaternionExtensions.FromEuler(axis.Multiply(angle)); // get the desired rotation
MVector3 dir = pos.Subtract(center); // find current direction relative to center
dir = rot.Multiply(dir); // rotate the direction
res.Position = center.Add(dir); // define new position
MQuaternion myRot = transform.Rotation;
res.Rotation = transform.Rotation.Multiply(MQuaternionExtensions.Inverse(myRot).Multiply(rot).Multiply(myRot));
return(res);
}
/// <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);
}
/// <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);
}
public static MQuaternion GetQuaternion(this MRotationConstraint rConstraint)
{
return(MQuaternionExtensions.FromEuler(rConstraint.GetVector3()));
}
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>
/// 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);
}
}
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));
}
/// <summary>
/// Returns all ik constraints which are violated (avove specified threshold)
/// </summary>
/// <param name="constraints"></param>
/// <param name="currentPosture"></param>
/// <returns></returns>
private List <MConstraint> GetViolatedIKConstraints(List <MConstraint> constraints, MAvatarPostureValues currentPosture)
{
List <MConstraint> violated = new List <MConstraint>();
if (constraints != null)
{
// Apply result posture values to the skeleton
skeletonAccess.SetChannelData(currentPosture);
string avatarID = currentPosture.AvatarID;
//Check each joint constraint
foreach (MConstraint mconstraint in constraints)
{
if (mconstraint.JointConstraint != null)
{
MJointConstraint endeffectorConstraint = mconstraint.JointConstraint;
//Skip if no gemometry constraint is defined
if (endeffectorConstraint.GeometryConstraint == null)
{
continue;
}
double distance = 0f;
double angularDistance = 0f;
//Default (parent to constraint is set)
if (endeffectorConstraint.GeometryConstraint.ParentToConstraint != null)
{
MVector3 position = endeffectorConstraint.GeometryConstraint.ParentToConstraint.Position;
MQuaternion rotation = endeffectorConstraint.GeometryConstraint.ParentToConstraint.Rotation;
switch (endeffectorConstraint.JointType)
{
case MJointType.LeftWrist:
distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftWrist));
angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftWrist));
break;
case MJointType.RightWrist:
distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightWrist));
angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightWrist));
break;
case MJointType.LeftBall:
distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftAnkle));
angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftAnkle));
break;
case MJointType.RightBall:
distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightAnkle));
angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightAnkle));
break;
case MJointType.PelvisCentre:
distance = MVector3Extensions.Distance(position, this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.PelvisCentre));
angularDistance = MQuaternionExtensions.Angle(rotation, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.PelvisCentre));
break;
}
}
//Legacy fallback mechanism -> Remove in future
else
{
MTranslationConstraint positionConstraint = endeffectorConstraint.GeometryConstraint.TranslationConstraint;
if (endeffectorConstraint.GeometryConstraint.TranslationConstraint != null)
{
switch (endeffectorConstraint.JointType)
{
case MJointType.LeftWrist:
distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftWrist));
break;
case MJointType.RightWrist:
distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightWrist));
break;
case MJointType.LeftBall:
distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.LeftAnkle));
break;
case MJointType.RightBall:
distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.RightAnkle));
break;
case MJointType.PelvisCentre:
distance = MVector3Extensions.Distance(new MVector3(positionConstraint.X(), positionConstraint.Y(), positionConstraint.Z()), this.skeletonAccess.GetGlobalJointPosition(avatarID, MJointType.PelvisCentre));
break;
}
}
//Handle the rotation constraint
if (endeffectorConstraint.GeometryConstraint.RotationConstraint != null)
{
MRotationConstraint rotationConstraint = endeffectorConstraint.GeometryConstraint.RotationConstraint;
//Compute a quaternion based on the euler angles
MQuaternion quaternion = MQuaternionExtensions.FromEuler(new MVector3(rotationConstraint.X(), rotationConstraint.Y(), rotationConstraint.Z()));
if (endeffectorConstraint.GeometryConstraint.ParentObjectID == null || endeffectorConstraint.GeometryConstraint.ParentObjectID == "")
{
switch (endeffectorConstraint.JointType)
{
case MJointType.LeftWrist:
angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftWrist));
break;
case MJointType.RightWrist:
angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightWrist));
break;
case MJointType.LeftBall:
angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.LeftAnkle));
break;
case MJointType.RightBall:
angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.RightAnkle));
break;
case MJointType.PelvisCentre:
angularDistance = MQuaternionExtensions.Angle(quaternion, this.skeletonAccess.GetGlobalJointRotation(avatarID, MJointType.PelvisCentre));
break;
}
}
}
}
//Check if solving is required
if (distance > this.PositionThreshold || angularDistance > this.RotationThreshold)
{
violated.Add(mconstraint);
}
}
}
}
return(violated);
}
/// <summary>
/// Method performs a local motion planning and tries to reach the specified goal position and rotation using the given velocity,angular velocity and time.
/// </summary>
/// <param name="velocity"></param>
/// <param name="angularVelocity"></param>
/// <param name="time"></param>
/// <param name="currentPosition"></param>
/// <param name="currentRotation"></param>
/// <param name="targetPosition"></param>
/// <param name="targetRotation"></param>
/// <returns></returns>
private MTransform DoLocalMotionPlanning(double velocity, double angularVelocity, TimeSpan time, MVector3 currentPosition, MQuaternion currentRotation, MVector3 targetPosition, MQuaternion targetRotation, bool collisionAvoidance)
{
//Create a new transform representing the result
MTransform result = new MTransform();
//Estimate the delta
MVector3 delta = targetPosition.Subtract(currentPosition);
//Determine the current delta angle
double angle = Math.Abs(MQuaternionExtensions.Angle(currentRotation, targetRotation));
//Determine the max translation delta and max angle in the current frame
double maxTranslationDelta = velocity * time.TotalSeconds;
double maxAngle = angularVelocity * time.TotalSeconds;
//Estimate the blend weight for the rotation and position
float rotationWeight = (float)Math.Min(1, maxAngle / angle);
float positionWeight = (float)Math.Min(1, maxTranslationDelta / delta.Magnitude());
//Limit the max translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
if (collisionAvoidance)
{
MVector3 collisionAvoidanceForce = this.ComputCollisionAvoidance(currentPosition, delta);
//if (collisionAvoidanceForce.Magnitude() > 0)
// MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_INFO, "Collision avoidance force: " + collisionAvoidanceForce.Magnitude());
//Add the collision avoidance force on top
delta = delta.Add(collisionAvoidanceForce);
//Limit the max translation
if (delta.Magnitude() >= maxTranslationDelta)
{
delta = delta.Normalize();
delta = delta.Multiply(maxTranslationDelta);
}
}
//Compute the new position
result.Position = currentPosition.Add(delta);
//Compute the new rotation by interpolating towards the target rotation
if (angularVelocity > 0)
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, rotationWeight);
}
//Use the rotation weight
else
{
result.Rotation = MQuaternionExtensions.Slerp(currentRotation, targetRotation, positionWeight);
}
//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 = 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>
/// 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);
}
/// <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);
}
/// <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>
/// Transforms a point from the global space to the local space of the MTransform
/// </summary>
/// <param name="transform"></param>
/// <param name="globalPosition"></param>
/// <returns></returns>
public static MVector3 InverseTransformPoint(this MTransform transform, MVector3 globalPosition)
{
return(MQuaternionExtensions.Inverse(transform.Rotation).Multiply(globalPosition.Subtract(transform.Position)));
}
/// <summary>
/// Transforms a rotation from global space into the local space of the MTransform
/// </summary>
/// <param name="transform"></param>
/// <param name="globalRotation"></param>
/// <returns></returns>
public static MQuaternion InverseTransformRotation(this MTransform transform, MQuaternion globalRotation)
{
return(MQuaternionExtensions.Inverse(transform.Rotation).Multiply(globalRotation));
}
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>()
};
//Cast to intermediate skeleton to get all functions
IntermediateSkeleton iS = this.SkeletonAccess as IntermediateSkeleton;
//Assign the approved posture of the last frame (proper finger rotations)
this.SkeletonAccess.SetChannelData(simulationState.Initial);
//First estimate the finger rotations
//------------------------------------------------------------------------------------------------------------------
//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.Release)
{
//Use all finger joint if in release mode
foreach (MJointType jointType in fingerJoints)
{
//Get the current rotation of the finger
if (!hand.CurrentFingerRotations.ContainsKey(jointType))
{
hand.CurrentFingerRotations.Add(jointType, iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, jointType));
}
else
{
hand.CurrentFingerRotations[jointType] = iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, jointType);
}
}
}
else
{
//Handle all joint constraints -> Use only the fingers that are constrained
foreach (MJointConstraint joint in hand.FinalPosture.JointConstraints)
{
//Skip if the joint is no finger joint
if (!this.IsFingerJoint(joint.JointType))
{
continue;
}
//Get the current rotation of the finger
if (!hand.CurrentFingerRotations.ContainsKey(joint.JointType))
{
hand.CurrentFingerRotations.Add(joint.JointType, iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, joint.JointType));
}
else
{
hand.CurrentFingerRotations[joint.JointType] = iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, joint.JointType);
}
}
}
}
//Perform the blending
//------------------------------------------------------------------------------------------------------------------
//Assign the approved posture of the last frame (proper finger rotations)
this.SkeletonAccess.SetChannelData(simulationState.Current);
//Handle each active hand
for (int i = this.ActiveHands.Count - 1; i >= 0; i--)
{
//Get the current hand
HandContainer hand = this.ActiveHands[i];
//Flag which indicates whether the fingers are positioned
bool positioned = true;
if (hand.Release)
{
//If in release mode again use all joints being finger joints
foreach (MJointType jointType in fingerJoints)
{
//Get the current rotation of the finger
MQuaternion currentRotation = hand.CurrentFingerRotations[jointType];
//The current rotation is the result of the last frame
MQuaternion desiredRotation = iS.GetLocalJointRotation(this.AvatarDescription.AvatarID, jointType);
//The weight used for blending
double weight = 0;
//The angle between the current rotation and the desired one
double angle = MQuaternionExtensions.Angle(currentRotation, desiredRotation);
//Compute the weight based on the elapsed time if the duration is set
if (hand.HasDuration)
{
weight = hand.Elapsed / hand.Duration;
}
//By default use the angular velocity
else
{
//The max allowed angle in this frame
double maxAngle = time * hand.AngularVelocity;
weight = Math.Min(1, maxAngle / angle);
}
//Compute the new rotation
MQuaternion newRotation = MQuaternionExtensions.Slerp(currentRotation, desiredRotation, (float)weight);
//Set the local joint rotation of the intermediate skeleton
iS.SetLocalJointRotation(this.AvatarDescription.AvatarID, jointType, newRotation);
//Goal criteria
if (angle > 0.1f)
{
positioned = false;
}
}
}
else
{
//Handle all joint constraints
foreach (MJointConstraint joint in hand.FinalPosture.JointConstraints)
{
//Skip if the joint is no finger joint
if (!this.IsFingerJoint(joint.JointType))
{
continue;
}
//Get the current rotation of the finger
MQuaternion currentRotation = hand.CurrentFingerRotations[joint.JointType];
//Get the desired rotation
MQuaternion desiredRotation = joint.GeometryConstraint.ParentToConstraint.Rotation;
//The weight used for blending
double weight = 0;
//The angle between the current rotation and the desired one
double angle = MQuaternionExtensions.Angle(currentRotation, desiredRotation);
//Compute the weight based on the elapsed time if the duration is set
if (hand.HasDuration)
{
weight = hand.Elapsed / hand.Duration;
}
//By default use the angular velocity
else
{
//The max allowed angle in this frame
double maxAngle = time * hand.AngularVelocity;
weight = Math.Min(1, maxAngle / angle);
}
//Compute the new rotation
MQuaternion newRotation = MQuaternionExtensions.Slerp(currentRotation, desiredRotation, (float)weight);
//Set the local joint rotation of the intermediate skeleton
iS.SetLocalJointRotation(this.AvatarDescription.AvatarID, joint.JointType, newRotation);
//Goal criteria
if (angle > 0.1f)
{
positioned = false;
}
}
}
//Provide event if positioned successfully
if (positioned && !hand.Positioned)
{
hand.Positioned = true;
result.Events.Add(new MSimulationEvent()
{
Name = "MoveFingersMMU",
Type = "FingersPositioned",
Reference = hand.Instruction.ID
});
}
//Increment the time
hand.Elapsed += (float)time;
}
//Recompute the posture given the performed changes
result.Posture = iS.RecomputeCurrentPostureValues(this.AvatarDescription.AvatarID);
//Return the simulation result for the given frame
return(result);
}