/// <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); }