/// <summary> /// This is the function called by the child thread. /// </summary> void ChildThreadLoop() { while (runningMPC) { if (toPause) { this.PauseThread(); } // Do Update Debug.LogWarning("Starting MPC Loop at pos: " + this.redirectionManager.currState.posReal); var watch = System.Diagnostics.Stopwatch.StartNew(); // start a timer MPCResult result = Plan(this.redirectionManager.currState, this.currSeg, this.planningHorizon); this.currAction = result.bestAction; watch.Stop(); // stop the timer var deltaT = watch.ElapsedMilliseconds; Debug.Log("best action: " + this.currAction.type + ", gain:" + this.currAction.gain + ", best cost: " + result.bestCost + ", duration: " + deltaT); //Thread.Sleep(1000); // use this if you want to be more stable } }
public MPCResult MPCRedirect(DataRecord.Data data, DataRecord.WayPointsReal[] wayPoints, int depth) { MPCResult result = new MPCResult { cost = 0, action = ActionTaken.Zero }; if (depth == 0) { return(result); } else { MPCResult tempResult = new MPCResult { }; MPCResult loopBestResult = new MPCResult { cost = Mathf.Infinity }; int matchNum = NearestWayPoint(data.realPosition, wayPoints); //foreach(var wayPoint in wayPoints) //{ // Debug.Log(wayPoint.realPosition); //} //loop action for (int i = 0; i < MPCResultNum; ++i) { int nearestNumTemp; float distanceCost; float parallelCost; float rotationAngle; Vector3 endPosition; Vector3 endDir; DataRecord.WayPointsReal[] tempWayPoints; DataRecord.Data tempPlayerData = new DataRecord.Data(); switch ((ActionTaken)i) { case ActionTaken.Zero: //foreach(var wayPoint in wayPoints) //{ // Debug.Log(wayPoint.realPosition); //} endPosition = ZeroAcition(wayPoints[matchNum], wayPoints[matchNum + 1], data.velocity); //Debug.Log(endPosition); nearestNumTemp = NearestWayPoint(endPosition, wayPoints); distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor); endDir = new Vector3(wayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, wayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z); parallelCost = ParallelWallCost(endDir, parallelFactor); tempPlayerData.realPosition = endPosition; tempPlayerData.velocity = data.velocity; tempResult.action = ActionTaken.Zero; tempResult.cost = distanceCost + parallelCost + MPCRedirect(tempPlayerData, wayPoints, depth - 1).cost; break; case ActionTaken.PositiveRotation: if (rotationCost > loopBestResult.cost) { break; } else if (wayPoints[matchNum].turnType != WayPoint.turnType.ninetyLeft && wayPoints[matchNum].turnType != WayPoint.turnType.ninetyRight) { break; } else { if (wayPoints[matchNum].turnType == WayPoint.turnType.ninetyRight) { rotationAngle = (rotateGainEnlarge - 1) * 90; tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, rotationAngle); endPosition = ZeroAcition(wayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity); nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints); distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor); endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z); parallelCost = ParallelWallCost(endDir, parallelFactor); tempPlayerData.realPosition = endPosition; tempPlayerData.velocity = data.velocity; tempResult.action = ActionTaken.PositiveRotation; tempResult.cost = distanceCost + parallelCost + rotationCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost; } else if (wayPoints[matchNum].turnType == WayPoint.turnType.ninetyLeft) { rotationAngle = 360 - (rotateGainEnlarge - 1) * 90; tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, rotationAngle); endPosition = ZeroAcition(wayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity); nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints); distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor); endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z); parallelCost = ParallelWallCost(endDir, parallelFactor); tempPlayerData.realPosition = endPosition; tempPlayerData.velocity = data.velocity; tempResult.action = ActionTaken.PositiveRotation; tempResult.cost = rotationCost + distanceCost + parallelCost + rotationCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost; } } break; case ActionTaken.NegativeRotation: if (rotationCost > loopBestResult.cost) { break; } else if (wayPoints[matchNum].turnType != WayPoint.turnType.ninetyLeft && wayPoints[matchNum].turnType != WayPoint.turnType.ninetyRight) { break; } else { if (wayPoints[matchNum].turnType == WayPoint.turnType.ninetyRight) { rotationAngle = 360 - (1 - rotateGainDecrease) * 90; tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, rotationAngle); endPosition = ZeroAcition(wayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity); nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints); distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor); endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z); parallelCost = ParallelWallCost(endDir, parallelFactor); Debug.Log(distanceCost); tempPlayerData.realPosition = endPosition; tempPlayerData.velocity = data.velocity; tempResult.action = ActionTaken.NegativeRotation; tempResult.cost = rotationCost + distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost; } else if (wayPoints[matchNum].turnType == WayPoint.turnType.ninetyLeft) { rotationAngle = (1 - rotateGainDecrease) * 90; tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, rotationAngle); endPosition = ZeroAcition(wayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity); nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints); distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor); endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z); parallelCost = ParallelWallCost(endDir, parallelFactor); tempPlayerData.realPosition = endPosition; tempPlayerData.velocity = data.velocity; tempResult.action = ActionTaken.NegativeRotation; tempResult.cost = rotationCost + distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost; } } break; case ActionTaken.PositiveReset: if (resetCost > loopBestResult.cost) { break; } else { tempWayPoints = RotateWayPoints(data.realPosition, wayPoints, 180); endPosition = ZeroAcition(tempWayPoints[matchNum], tempWayPoints[matchNum + 1], data.velocity); nearestNumTemp = NearestWayPoint(endPosition, tempWayPoints); distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor); endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z); parallelCost = ParallelWallCost(endDir, parallelFactor); tempPlayerData.realPosition = endPosition; tempPlayerData.velocity = data.velocity; tempResult.action = ActionTaken.PositiveReset; tempResult.cost = distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost + resetCost; break; } //case ActionTaken.NegativeReset: // if (resetCost > loopBestResult.cost) break; // else break; case ActionTaken.ClockwiseCurvature: //Debug.Log("Here"+ loopBestResult.cost); if (curvatureCost > loopBestResult.cost) { break; } else { tempWayPoints = wayPoints; endPosition = tempWayPoints[matchNum].realPosition; int tempMatchNum = matchNum; nearestNumTemp = matchNum; float deltaPos = data.velocity * timeHorizon / curvatureCalFrequence; float deltaAngle = 360 - (deltaPos * 360 / (2 * Mathf.PI * curvatureRadius)); for (int j = 0; j < curvatureCalFrequence; j++) { endPosition = GeneralVector3.LineDistance(endPosition, tempWayPoints[tempMatchNum + 1].realPosition, deltaPos); if (Vector3.Distance(endPosition, tempWayPoints[tempMatchNum + 1].realPosition) < distanceThreshlod) { tempMatchNum++; nearestNumTemp = tempMatchNum; } tempWayPoints = RotateWayPoints(endPosition, tempWayPoints, deltaAngle); } distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor); endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z); parallelCost = ParallelWallCost(endDir, parallelFactor); //Debug.Log(distanceCost); tempPlayerData.realPosition = endPosition; tempPlayerData.velocity = data.velocity; tempResult.action = ActionTaken.ClockwiseCurvature; tempResult.cost = distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost + curvatureCost; break; } case ActionTaken.CounterClockwiseCurvature: if (curvatureCost > loopBestResult.cost) { break; } else { tempWayPoints = wayPoints; endPosition = tempWayPoints[matchNum].realPosition; int tempMatchNum = matchNum; nearestNumTemp = matchNum; float deltaPos = data.velocity * timeHorizon / curvatureCalFrequence; float deltaAngle = deltaPos * 360 / (2 * Mathf.PI * curvatureRadius); for (int j = 0; j < curvatureCalFrequence; j++) { endPosition = GeneralVector3.LineDistance(endPosition, tempWayPoints[tempMatchNum + 1].realPosition, deltaPos); if (Vector3.Distance(endPosition, tempWayPoints[tempMatchNum + 1].realPosition) < distanceThreshlod) { tempMatchNum++; nearestNumTemp = tempMatchNum; } tempWayPoints = RotateWayPoints(endPosition, tempWayPoints, deltaAngle); } distanceCost = TowardWallCost(endPosition, width, length, distanceCenterFactor); endDir = new Vector3(tempWayPoints[nearestNumTemp + 1].realPosition.x - endPosition.x, 0, tempWayPoints[nearestNumTemp + 1].realPosition.z - endPosition.z); parallelCost = ParallelWallCost(endDir, parallelFactor); tempPlayerData.realPosition = endPosition; tempPlayerData.velocity = data.velocity; tempResult.action = ActionTaken.CounterClockwiseCurvature; tempResult.cost = distanceCost + parallelCost + MPCRedirect(tempPlayerData, tempWayPoints, depth - 1).cost + curvatureCost; break; } default: break; } if (tempResult.cost < loopBestResult.cost) { loopBestResult = tempResult; } } //Debug.Log(loopBestResult.cost); result = loopBestResult; //multply declinefactor if (depth != timeDepth) { result.cost *= declineFactor; } //Debug.Log(result.cost); return(result); } }
/// <summary> /// The main MPC K-stage forward planning, this is basically depth-first tree search with branch cutting. /// </summary> /// <param name="state"></param> current state /// <param name="currSeg"></param> current segment of the user /// <param name="depth"></param> the planning depth /// <returns></returns> public MPCResult Plan(RedirectionManager.State state, Segment currSeg, int depth) { //Debug.Log("Plan called"); if (depth == 0) { return new MPCResult { bestCost = 0, bestAction = { } } } ; else { float bestCost = Mathf.Infinity; Action bestAction = GetZeroAction(); //Debug.Log("Plan depth="+depth+" pos="+state.posReal); float cost; List <Action> allowedActions = GetAllowedActions(currSeg); //Debug.Log("GetAllowedActions"); foreach (Action a in allowedActions) { //Debug.Log("depth=" + depth + ", action=" + a.type+", gain="+a.gain); cost = 0f; if (a.cost < bestCost) { cost += a.cost; List <Segment> nextSegments = currSeg.GetNextSegments(state); //Debug.Log("GetNextSegments"); foreach (var nextSeg in nextSegments) { //Debug.Log("?????????? " + state.dirReal); RedirectionManager.State nextState = ApplyStateUpdate(state, a, nextSeg); //Debug.Log("pos="+state.posReal+" next pos="+nextState.posReal); float stateCost = GetStateCost(nextState); cost += stateCost * nextSeg.proba; //Debug.Log("summed cost= " + cost + ", cost of next state: " + stateCost); if (cost >= bestCost) { //Debug.Log("CUTOFF cost=" + cost + ", bestcost=" + bestCost); break; } if (depth > 0) { MPCResult nextResult = Plan(nextState, nextSeg, depth - 1); cost += declineFactor * nextSeg.proba * nextResult.bestCost; //Debug.Log("Depth=" + depth + ", action: " + a.type + " gain: "+a.gain+ // ", final cost=" + cost + ", next bestcost=" + nextResult.bestCost); } } if (cost < bestCost) { //Debug.Log("update best cost, new best=" + cost + ", old bestcost=" + bestCost+", action: " // +bestAction.type+", gain="+bestAction.gain); bestCost = cost; bestAction = a; } } else { //Debug.Log("Depth=" + depth + " action " + a.type+ ":"+a.gain + " cutoff with cost " + a.cost); } } //if (depth==4) //{ // Debug.Log("Depth=" + depth + " final best cost: " + bestCost + ", best action: " + bestAction.type // + ", gain: " + bestAction.gain + " at pos=" + state.posReal + " dir=" + state.dirReal); //} return(new MPCResult { bestAction = bestAction, bestCost = bestCost }); } }