/// <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>
/// 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>
/// Returns the translation distance to move the hand from the initial state to the current state
/// </summary>
/// <param name="type"></param>
/// <returns></returns>
private float GetHandDistance(HandType type)
{
MVector3 targetHandPosition = this.GetGlobalPosition(simulationState.Current, type);
MVector3 currentHandPosition = this.GetGlobalPosition(simulationState.Initial, type);
return(targetHandPosition.Subtract(currentHandPosition).Magnitude());
}
/// <summary>
/// Implementation of the check prerequisites method which is used by the co-simulation to determine whether the MMU can be started
/// </summary>
/// <param name="instruction"></param>
/// <returns></returns>
public override MBoolResponse CheckPrerequisites(MInstruction instruction)
{
//Get the min distance parameter
if (instruction.Properties != null && instruction.Properties.ContainsKey("MinDistance"))
{
instruction.Properties.GetValue(out minReachDistance, "MinDistance");
}
else
{
minReachDistance = minDistanceDefault;
}
if (instruction.Properties.ContainsKey("TargetID"))
{
MSceneObject sceneObject = this.SceneAccess.GetSceneObjectByID(instruction.Properties["TargetID"]);
MAvatar avatar = this.SceneAccess.GetAvatarByID(this.AvatarDescription.AvatarID);
if (sceneObject != null && avatar != null)
{
this.SkeletonAccess.SetChannelData(avatar.PostureValues);
//Get the root position
MVector3 rootPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.PelvisCentre);
rootPosition.Y = 0;
//Get the object position
MVector3 objectPosition = new MVector3(sceneObject.Transform.Position.X, 0, sceneObject.Transform.Position.Z);
//Compute the distance between root and object
float distance = rootPosition.Subtract(objectPosition).Magnitude();
//Check if below distance
if (distance < this.minReachDistance)
{
if (this.debug)
{
Logger.Log(Log_level.L_DEBUG, $"Check prerequisites of reach successfull! Distance: {distance}/{minReachDistance}");
}
return(new MBoolResponse(true));
}
else
{
if (this.debug)
{
Logger.Log(Log_level.L_DEBUG, $"Check prerequisites of reach failed! Distance: {distance}/{minReachDistance}");
}
return(new MBoolResponse(false));
}
}
}
return(new MBoolResponse(true));
}
/// <summary>
/// Returns the avatars in range of the specified position
/// </summary>
/// <param name="position"></param>
/// <param name="distance"></param>
/// <returns></returns>
public List <MAvatar> GetAvatarsInRange(MVector3 position, double distance)
{
List <MAvatar> result = new List <MAvatar>();
foreach (MAvatar avatar in this.GetAvatars())
{
MVector3 avatarPosition = new MVector3(avatar.PostureValues.PostureData[0], avatar.PostureValues.PostureData[1], avatar.PostureValues.PostureData[2]);
if (avatarPosition.Subtract(position).Magnitude() <= distance)
{
result.Add(avatar);
}
}
return(result);
}
/// <summary>
/// Computes the new (desired) hand position considering the offset of the collider (to avoid self-collisions)
/// </summary>
/// <param name="targetHandPosition"></param>
/// <param name="currentPosture"></param>
/// <returns></returns>
private MVector3 ComputeNewPositionWithOffset(MVector3 targetHandPosition, MAvatarPostureValues currentPosture)
{
//Optionally ensure that the object does not intersect the avatar
MCollider collider = this.SceneAccess.GetColliderById(this.objectTransform.ID);
//Determine the offset based on the respective collider
float offset = 0;
if (collider.SphereColliderProperties != null)
{
offset = (float)collider.SphereColliderProperties.Radius;
}
if (collider.BoxColliderProperties != null)
{
offset = (float)collider.BoxColliderProperties.Size.Magnitude();
}
if (collider.CapsuleColliderProperties != null)
{
offset = Math.Max((float)collider.CapsuleColliderProperties.Height, (float)collider.CapsuleColliderProperties.Radius);
}
//The offset could be also dynamically determined (using the mesh intersection distance or using Physics Compute Pentration in unity)
this.SkeletonAccess.SetChannelData(currentPosture);
//Get the shoulder positions
MVector3 leftShoulderPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.LeftShoulder);
MVector3 rightShoulderPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.RightShoulder);
//Compute the direction vector pointing from the avatar towards the respective hand
MVector3 dir = new MVector3(0, 0, 0);
switch (this.handJoint)
{
case MJointType.LeftWrist:
dir = leftShoulderPosition.Subtract(rightShoulderPosition).Normalize();
break;
case MJointType.RightWrist:
dir = rightShoulderPosition.Subtract(leftShoulderPosition).Normalize();
break;
}
//Add an offset on top of the position
return(targetHandPosition.Add(dir.Multiply(offset)));
}
/// <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);
}
/// <summary>
/// Computes the root velocity of the avatar given the initial and current state
/// </summary>
/// <param name="time"></param>
/// <returns></returns>
private float ComputeRootVelocity(double time, MSimulationState simulationState)
{
float velocity = 0;
if (this.rootPositionLastFrame != null)
{
MVector3 currentRootPosition = new MVector3(simulationState.Initial.PostureData[0], 0, simulationState.Initial.PostureData[2]);
//Estimate the root velocity
velocity = (rootPositionLastFrame.Subtract(currentRootPosition)).Magnitude() / (float)time;
}
Console.WriteLine("Root velocity: " + velocity);
this.rootPositionLastFrame = new MVector3(simulationState.Initial.PostureData[0], 0, simulationState.Initial.PostureData[2]);
return(velocity);
}
/// <summary>
/// Sets the root position of an avatar. In addition, this function re-aligns the pelvis
/// and the root joint. Meaning, the root joint is set to the target position and
/// the translation animation of the pelvis is removed.
/// </summary>
/// <param name="avatarId"></param>
/// <param name="position"></param>
public void SetRootPosition(string avatarId, MVector3 position)
{
MAvatarPostureValues values = this.GetCurrentPostureValues(avatarId);
position = position.Subtract(this.hierarchies[avatarId].GetMJoint().Position);
// Set root position
values.PostureData[0] = position.X;
values.PostureData[1] = position.Y;
values.PostureData[2] = position.Z;
// reset pelvis position to be positioned above the root
values.PostureData[7] = 0;
values.PostureData[8] = 0;
values.PostureData[9] = 0;
this.hierarchies[values.AvatarID].SetAvatarPostureValues(values, this.animatedJoints[values.AvatarID]);
// I avoid using the SetChannelData here, to not overwrite the last set channel values.
// this.SetChannelData(values);
}
/// <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>
/// 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)));
}
private double Distance(MVector3 v1, MVector3 v2)
{
return(v1.Subtract(v2).Magnitude());
}
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 = new List <MSimulationEvent>(),
Constraints = simulationState.Constraints ?? new List <MConstraint>(),
SceneManipulations = new List <MSceneManipulation>()
};
this.SkeletonAccess.SetChannelData(simulationState.Current.Copy());
// Target position we want to transform to
MVector3 targetPos = this.targetTransform.Position;
// Fully body posture. Only Pelvis Center (first joint) has to be manipulated.
List <double> posture = simulationState.Current.PostureData;
// Current position and distance to target.
MVector3 currentPos = this.SkeletonAccess.GetRootPosition(simulationState.Initial.AvatarID);
MVector3 distance = targetPos.Subtract(currentPos);
// Current rotation and rotational diff to target
MQuaternion currentRot = this.SkeletonAccess.GetRootRotation(simulationState.Initial.AvatarID);
MQuaternion targetRot = this.targetTransform.Rotation;
MVector3 newPos;
MVector3 deltaDistance = distance.Clone();
if (this.velocity > 0)
{
deltaDistance = distance.Normalize().Multiply(this.velocity * time);
Console.WriteLine("Delta v: " + deltaDistance.Magnitude() + " " + time + " " + this.velocity);
}
// If no velocity set or distance very close, directly morph to target position and rotation.
if (this.velocity <= 0 || distance.Magnitude() < deltaDistance.Magnitude())
{
newPos = targetPos;
// Set rotation
//posture[3] = this.targetTransform.Rotation.W;
//posture[4] = this.targetTransform.Rotation.X;
//posture[5] = this.targetTransform.Rotation.Y;
//posture[6] = this.targetTransform.Rotation.Z;
// Add end event.
Console.WriteLine("Finished with vel " + this.velocity + " at " + distance.Magnitude());
result.Events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
else // if velocity > 0 and distance sufficiently large, we should apply linear translation with the provided velocity.
{
newPos = currentPos.Add(deltaDistance);
Console.WriteLine("Target Location: " + this.targetTransform.Position + " " + currentPos + " " + distance + " " + deltaDistance + " " + newPos);
}
Console.WriteLine("newposrot: " + newPos + " " + targetRot);
this.SkeletonAccess.SetRootPosition(simulationState.Current.AvatarID, newPos);
this.SkeletonAccess.SetRootRotation(simulationState.Current.AvatarID, targetRot);
result.Posture = this.SkeletonAccess.GetCurrentPostureValues(simulationState.Current.AvatarID);
Console.WriteLine("Frame : " + result.Posture.PostureData[0] + " " + result.Posture.PostureData[1] + " " + result.Posture.PostureData[2] + " " + result.Posture.PostureData[3] + " " + result.Posture.PostureData[4] + " " + result.Posture.PostureData[5] + " " + result.Posture.PostureData[6] + " ");
//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>
/// Computes a path given a start and end position
/// </summary>
/// <param name="from"></param>
/// <param name="target"></param>
/// <param name="filterSceneObjects"></param>
/// <returns></returns>
private MotionTrajectory2D ComputePath(Vector2 from, Vector2 target, bool filterSceneObjects = true)
{
List <MTransform> computedPath = new List <MTransform>();
//Only plan path if distance is above threshold
if ((from - target).magnitude > 0.2f)
{
try
{
//Get all scene objects from the scene
List <MSceneObject> sceneObjects = this.SceneAccess.GetSceneObjects();
///Remove scene objects in range if filtering is enabled
if (filterSceneObjects)
{
MVector3 hipPosition = this.GetGlobalPosition(this.simulationState.Initial, MJointType.PelvisCentre);
MVector3 leftHandPosition = this.GetGlobalPosition(this.simulationState.Initial, MJointType.LeftWrist);
MVector3 rightHandPosition = this.GetGlobalPosition(this.simulationState.Initial, MJointType.RightWrist);
for (int i = sceneObjects.Count - 1; i >= 0; i--)
{
MVector3 sceneObjectPosition = sceneObjects[i].Transform.Position;
float hipDist = (sceneObjectPosition.Subtract(hipPosition)).Magnitude();
float lhandDist = (leftHandPosition.Subtract(sceneObjectPosition)).Magnitude();
float rhandDist = (rightHandPosition.Subtract(sceneObjectPosition)).Magnitude();
if (lhandDist < 0.5f || rhandDist < 0.5f)
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_DEBUG, $"Removing scene object {sceneObjects[i].Name}, that is not included in path planning.");
sceneObjects.RemoveAt(i);
}
}
}
//Compute the path using the path planning service
MPathConstraint result = ServiceAccess.PathPlanningService.ComputePath(
new MVector()
{
Values = new List <double>()
{
from.x, from.y
}
},
new MVector()
{
Values = new List <double>()
{
target.x, target.y
}
},
sceneObjects,
new Dictionary <string, string>()
{
{ "mode", "2D" },
{ "time", Serialization.ToJsonString(1.0f) },
{ "radius", Serialization.ToJsonString(0.3f) },
{ "height", Serialization.ToJsonString(0.5f) },
});
//Get the computed path
if (result.PolygonPoints.Count > 0)
{
if (result.PolygonPoints[0].ParentToConstraint != null)
{
computedPath = result.PolygonPoints.Select(s => new MTransform()
{
Position = new MVector3(s.ParentToConstraint.Position.X, 0, s.ParentToConstraint.Position.Z)
}).ToList();
}
else
{
// TODO: Legacy support. Remove in a future version
computedPath = result.PolygonPoints.Select(s => new MTransform()
{
Position = new MVector3(s.TranslationConstraint.X(), 0, s.TranslationConstraint.Z())
}).ToList();
}
}
}
catch (Exception e)
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, "Problem at computing path using service " + e.Message + " " + e.StackTrace);
//In case of an exception return the straight line
//To do use an optional flag to adjust the desired behavior, e.g. should an error be returned instead
if (this.useStraightLineIfNoPath)
{
computedPath = new List <MTransform>()
{
new MTransform()
{
Position = new MVector3(from.x, 0, from.y)
},
new MTransform()
{
Position = new MVector3((from.x + target.x) / 2, 0, (from.y + target.y) / 2)
},
new MTransform()
{
Position = new MVector3(target.x, 0, target.y)
},
};
}
}
finally
{
if (this.useStraightLineIfNoPath && computedPath.Count == 0)
{
computedPath = new List <MTransform>()
{
new MTransform()
{
Position = new MVector3(from.x, 0, from.y)
},
new MTransform()
{
Position = new MVector3((from.x + target.x) / 2, 0, (from.y + target.y) / 2)
},
new MTransform()
{
Position = new MVector3(target.x, 0, target.y)
},
};
}
}
}
//If really close to goal -> No detailed planning is required
else
{
computedPath = new List <MTransform>()
{
new MTransform()
{
Position = new MVector3(from.x, 0, from.y)
},
new MTransform()
{
Position = new MVector3((from.x + target.x) / 2, 0, (from.y + target.y) / 2)
},
new MTransform()
{
Position = new MVector3(target.x, 0, target.y)
},
};
}
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_DEBUG, "Computed path elements: " + computedPath.Count);
if (computedPath.Count == 0)
{
return(null);
}
//Create a motion trajectory from the path
return(new MotionTrajectory2D(computedPath, this.Velocity));
}
/// <summary>
/// Basic euclidean distance function for float arrays
/// </summary>
/// <param name="vector1"></param>
/// <param name="vector2"></param>
/// <returns></returns>
private static float EuclideanDistance(MVector3 vector1, MVector3 vector2)
{
MVector3 diff = vector1.Subtract(vector2);
return(diff.Magnitude());
}
/// <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);
}
