/// <summary>
/// Estimates the next waypoint of a given path.
/// Returns the first point which is farer away than the specified distance
/// </summary>
/// <param name="globalPath"></param>
/// <param name="currentPosition"></param>
/// <param name="horizon">The time horizon for planning</param>
/// <returns></returns>
private Vector2 GetNextWaypoint(MotionTrajectory2D globalPath, Vector2 currentPosition, TimeSpan horizon)
{
Vector2 waypoint = currentPosition;
TimeSpan time = this.EstimateCurrentTime(currentPosition);
//Determine the next pose
TimedPose2D pose = this.trajectory.Poses.Last();
//Only interpolate if time is not exceeded
if (time + horizon < pose.Time)
{
pose = this.trajectory.GetPose(time + horizon);
}
//Set the waypoint
waypoint = new Vector2(pose.Position.x, pose.Position.y);
return(waypoint);
}
/// <summary>
/// Method does the local steering
/// </summary>
/// <param name="time">The planning time for the present frame</param>
private void FollowPathRootTransform(float time)
{
//Use the root transform
Vector2 currentPosition = new Vector2(transform.position.x, transform.position.z);
Quaternion currentRotation = transform.rotation;
//Check if target position has changed
float dist = (this.lastGoalPosition - this.goalPosition).magnitude;
//Estimate the distance to the goal
float goalDistance = (currentPosition - this.goalPosition).magnitude;
bool replanPath = false;
//Check if the target object has changed -> Replanning required
if (dist > 0.1f)
{
this.goalPosition = new Vector2(this.goalPosition.x, this.goalPosition.y);
replanPath = true;
}
//Check if replanning is enforced by replanning time
else
{
replanPath = goalDistance > 0.5f && replanningTime > 0 && elapsedTime.TotalMilliseconds > 0 && (int)elapsedTime.TotalMilliseconds % this.replanningTime == 0;
}
//Optionally do reactive replanning
if (replanPath)
{
//Compute a new path
this.trajectory = this.ComputePath(new Vector2(transform.position.x, transform.position.z), this.goalPosition);
//Get the discrete poses from the path
this.discretePoses = this.trajectory.SampleFrames(60);
//Reset index and first frame flag
this.currentIndex = 0;
this.firstFrame = true;
//Create a visualization of the trajectory (drawing calls)
this.CreateTrajectoryVisualization();
}
//Get the next waypoint
Vector2 nextWaypoint = this.GetNextWaypoint(this.trajectory, currentPosition, this.TimeHorizon);
//Use the goal position directly if below threshold
if (goalDistance < 0.5)
{
nextWaypoint = new Vector2(this.goalPosition.x, this.goalPosition.y);
}
switch (this.state)
{
//Rotate towards the target before starting
case WalkState.Starting:
//Call the method of the dhm
if (this.OrientateTowards(nextWaypoint, this.timespan, 5))
{
//Change the state if finished
this.state = WalkState.Walking;
}
break;
//The main state during walking
case WalkState.Walking:
//Determine the next target position
Vector2 nextTarget = this.GetNextWaypoint(this.trajectory, currentPosition, this.TimeHorizon);
//Check if the goal distance is below a defined threhsold -> Set the next target to goal position
if (goalDistance < 0.5)
{
nextTarget = new Vector2(this.goalPosition.x, this.goalPosition.y);
}
//Determine the trajectory direction
Vector2 trajectoryDirection = (nextTarget - currentPosition);
//Estimate the max distance allowed
float travelDistance = time * this.Velocity;
//Compute the new position
Vector2 nextPosition = currentPosition + trajectoryDirection.normalized * travelDistance;
//Update the position and rotation according to the animation
this.transform.position = this.animator.rootPosition;
this.transform.rotation = this.animator.rootRotation;
//The current direction vector
Vector3 currentDirection = (this.transform.rotation * Vector3.forward);
currentDirection.y = 0;
//Estimate the current angle mismatch
float currentDeltaAngle = Vector3.Angle(currentDirection, new Vector3(trajectoryDirection.x, 0, trajectoryDirection.y));
//Rotate to direction
//Adjust the rotation
//Only adjust the rotation if a sufficient distance to the goal can be encountred
//In case the distance is too low -> Turning in place is used instead
if (Math.Abs(currentDeltaAngle) > 1e-5f && goalDistance > 0.3f)
{
float maxAngle = time * this.AngularVelocity;
float goalDelta = Math.Abs(currentDeltaAngle);
float delta = Math.Min(maxAngle, goalDelta);
Vector3 right = (this.transform.rotation * Vector3.right);
Vector3 left = (this.transform.rotation * Vector3.left);
//Determine the sign of the angle
float sign = -1;
if (Vector3.Angle(right, new Vector3(trajectoryDirection.x, 0, trajectoryDirection.y)) < Vector3.Angle(left, new Vector3(trajectoryDirection.x, 0, trajectoryDirection.y)))
{
sign = 1;
}
this.transform.rotation = Quaternion.Euler(0, sign * delta, 0) * this.transform.rotation;
}
//Update the animation -> Set the flots of the animator component to control the animation tree
this.animator.SetFloat("Velocity", ((nextPosition - currentPosition).magnitude / time));
this.animator.SetFloat("Direction", 0.5f);
//Check if target reached
if ((nextPosition - this.goalPosition).magnitude < this.goalAccuracy)
{
//Set velocity to zero
this.animator.SetFloat("Velocity", 0f);
//Check velocity threshold if defined
if (!useVelocityStoppingThreshold || (this.leftFootAnimationTracker.Velocity < velocityStoppingThreshold && this.rightFootAnimationTracker.Velocity < velocityStoppingThreshold))
{
//Go to finishing state and perform further reorientation
if (this.useTargetOrientation)
{
this.state = WalkState.Finishing;
}
//Finish and move to idle
else
{
this.state = WalkState.Idle;
this.events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
}
}
break;
//The finishing state which performs the final fine-grained rotation
case WalkState.Finishing:
//Get the ttarget transform
MTransform targetTransform = this.GetTarget();
MVector3 forward = targetTransform.Rotation.Multiply(new MVector3(0, 0, 1));
Vector2 forward2 = new Vector2((float)forward.X, (float)forward.Z).normalized;
bool validOrientation = false;
if (this.OrientateTowards(goalPosition + forward2 * 10f, this.timespan, 5))
{
validOrientation = true;
}
//Only finish if velocity is close to zero
if (validOrientation)
{
//Set to finished -> Reset state machine
this.state = WalkState.Idle;
//Add finished event
this.events.Add(new MSimulationEvent(this.instruction.Name, mmiConstants.MSimulationEvent_End, this.instruction.ID));
}
break;
}
}
public override MBoolResponse AssignInstruction(MInstruction instruction, MSimulationState simulationState)
{
//Create a response
MBoolResponse response = new MBoolResponse(true);
//Set default values
this.abort = false;
this.filterSceneObjects = true;
//Assign the simulation state
this.simulationState = simulationState;
//Assign the instruction
this.instruction = instruction;
//Reset the elapsed time
this.elapsedTime = TimeSpan.Zero;
///Parse the properties
bool requiredPropertiesSet = this.ParseProperties(instruction);
//Check if all required properties have been set
if (!requiredPropertiesSet)
{
response.LogData = new List <string>()
{
"Properties are not defined -> cannot start the MMU"
};
response.Successful = false;
return(response);
}
//Get the target transform
MTransform targetTransform = this.GetTarget();
if (targetTransform == null)
{
response.Successful = false;
response.LogData = new List <string>()
{
"Problem at fetching the target transform!"
};
return(response);
}
//Flag indicates wheather a predefined trajectory should be used
bool usePredefinedTrajectory = false;
//Execute instructions on main thread
this.ExecuteOnMainThread(() =>
{
//Set the channel data of the current simulation state
this.SkeletonAccess.SetChannelData(this.simulationState.Current);
//Get the root position and rotation
MVector3 rootPosition = this.SkeletonAccess.GetGlobalJointPosition(this.AvatarDescription.AvatarID, MJointType.PelvisCentre);
MQuaternion rootRotation = this.SkeletonAccess.GetGlobalJointRotation(this.AvatarDescription.AvatarID, MJointType.PelvisCentre);
//Extract the start position
Vector2 startPosition = new Vector2((float)rootPosition.X, (float)rootPosition.Z);
//Get the goal position
this.goalPosition = new Vector2((float)targetTransform.Position.X, (float)targetTransform.Position.Z);
this.lastGoalPosition = this.goalPosition;
//Fetch the trajectory if available
if (instruction.Properties.ContainsKey("Trajectory"))
{
try
{
//Get the path constraint
MPathConstraint pathConstraint = instruction.Constraints.Find(s => s.ID == instruction.Properties["Trajectory"]).PathConstraint;
//Get the actual trajectory from the path constraint
List <Vector2> pointList = pathConstraint.GetVector2List();
//Estimate the distance between the start point and first point of trajectory
float distance = (startPosition - pointList[0]).magnitude;
//If distance to first point of trajectory is below threshold -> Directly connect the start point with the first point of the trajectory
if (distance < 0.5)
{
pointList.Insert(0, startPosition);
trajectory = new MotionTrajectory2D(pointList, this.Velocity);
}
else
{
//Compute a path to the trajectory start location
this.trajectory = this.ComputePath(startPosition, pointList[0], this.filterSceneObjects);
//Add the defined trajectory
this.trajectory.Append(pointList);
//Finally estimate the timestamps based on the velocity
this.trajectory.EstimateTimestamps(this.Velocity);
}
//Set flag that no more planning is required
usePredefinedTrajectory = true;
}
catch (Exception e)
{
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, "UnityLocomotionMMU: Cannot parse trajectory -> plan new one: " + e.Message + " " + e.StackTrace);
}
}
//Only plan the path if no predefined trajectory should be used
if (!usePredefinedTrajectory)
{
//Compute a path which considers the indivudal path goals and the constraints
this.trajectory = this.ComputePath(startPosition, this.goalPosition, filterSceneObjects);
}
if (this.trajectory == null || this.trajectory.Poses.Count == 0)
{
this.abort = true;
MMICSharp.Adapter.Logger.Log(MMICSharp.Adapter.Log_level.L_ERROR, $"No path found in assign instruction. Aborting MMU.");
response.LogData = new List <string>()
{
"No path found"
};
response.Successful = false;
}
//Valid path found
if (this.trajectory != null && this.trajectory.Poses.Count > 0)
{
//Create the visualization data for the trajectory (drawing calls)
this.CreateTrajectoryVisualization();
//Set the speed of the animator
this.animator.speed = this.Velocity / 2.0f;
//Reset the goal direction
this.goalPosition = new Vector2();
//Update the animation
this.animator.SetFloat("Velocity", 0);
this.animator.SetFloat("Direction", 0.5f);
//Reset the current index
this.currentIndex = 0;
//Set the internal fields
this.transform.position = new Vector3((float)rootPosition.X, 0, (float)rootPosition.Z);
// In the new intermediate skeleton definition, x is pointing backwards, y up, z right.
this.transform.rotation = Quaternion.Euler(0, new Quaternion((float)rootRotation.X, (float)rootRotation.Y, (float)rootRotation.Z, (float)rootRotation.W).eulerAngles.y - 90.0f, 0);
//Get the discrete poses representing the trajectory
this.discretePoses = this.trajectory.SampleFrames(60);
//Set state to starting
this.state = WalkState.Starting;
//Set flag which indicates the start
this.firstFrame = true;
//Reset the foot trackers
this.leftFootAnimationTracker.Reset();
this.rightFootAnimationTracker.Reset();
}
});
return(response);
}
