private static DS[] PredUnionSelf(DS u, AnytimeDStarState aS) { var _ = u.Neighbors(aS.Ops, aS.ss, false); _.AddFirst(u); return(_.ToArray( )); }
private static Path <SS, DS> MoveAgent(AnytimeDStarState aS) { DS next = ArgMin(sp => c(aS.Sstart, sp, aS) + g(sp, aS), Succ(aS.Sstart, aS)); if (g(next, aS) == Metric.Infinity) { return(null); } Operator <SS, DS> Op = null; foreach (var action in aS.Ops) { if (action.IsValidOn(aS.Sstart, aS.ss, false) && action.PerformOn(aS.Sstart, aS.ss).First( ).Equals(next)) { Op = action; break; } } if (Op == null) { return(null); } else { aS.Sstart = next; return(Operator <SS, DS> .MakePath(Op)); } }
private static bool IsSignificantChange(DS Change, AnytimeDStarState aS) { //return true; return(aS.h(Change, aS.Sgoal) < aS.h(aS.Sstart, aS.Sgoal) && aS.h(Change, aS.Sstart) < aS.h(aS.Sstart, aS.Sgoal) && aS.h(Change, aS.Sstart) + aS.h(Change, aS.Sgoal) < SignifcanceRatio *aS.h(aS.Sstart, aS.Sgoal)); }
public static IEnumerable <Path <SS, DS> > AnytimeDStarLiteLookahead( AnytimeDStarState aS, int ComputationTime) { aS.RHS = new Dictionary <string, Metric>( ); aS.G = new Dictionary <string, Metric>( ); g(aS.Sstart, Metric.Infinity, aS); rhs(aS.Sstart, Metric.Infinity, aS); g(aS.Sgoal, Metric.Infinity, aS); rhs(aS.Sgoal, Metric.Zero, aS); aS.epsilon = epsilon0; aS.CLOSED = new HashSet <DS>( ); aS.OPEN = new PriorityQueue <SvenKey, DS>( ); aS.INCONS = new HashSet <DS>( ); aS.OPEN.Enqueue(key(aS.Sgoal, aS), aS.Sgoal); Status status; while (aS.Sstart != aS.Sgoal) { status = ComputeorImprovePath(aS); yield return(MoveAgent(aS)); bool SignficantChanges = false; if (aS.ss.HasRecentChanges()) { foreach (var u in aS.ss.Changes) { SignficantChanges |= IsSignificantChange(u, aS); foreach (var v in Succ(u, aS)) { UpdateState(u, aS); UpdateState(v, aS); } } aS.ss.Changes.Clear( ); } if (SignficantChanges) { aS.epsilon += epsilonIncreaseFactor; if (aS.epsilon > epsilon0) { aS.epsilon = epsilon0; } } else if (aS.epsilon > Metric.One) { aS.epsilon -= epsilonDecreaaseFactor; if (aS.epsilon < Metric.One) { aS.epsilon = Metric.One; } } MoveINCONSintoOPENAndResort(aS); aS.CLOSED = new HashSet <DS>( ); } yield break; }
private static void g(DS ds, Metric value, AnytimeDStarState aS) { string key = ds.ToString( ); if (aS.G.ContainsKey(key)) { aS.G.Remove(key); } aS.G.Add(key, value); }
public static Metric c(DS s1, DS s2, AnytimeDStarState aS) { if (s1.IsInvalid(aS.ss, false) || s2.IsInvalid(aS.ss, false)) { return(Metric.Infinity); } else { return(aS.h(s1, s2)); } }
public RealTimeAnytimeDStar(Heuristic <SS, DS> H, Transformer <SS, DS> Transformer, StateVisualizer <SS, DS> sv, ChooseMethodStep sm, double Ratio) { this.Transformer = Transformer; this.H = H; this.aS = new RealTimeAnytimeDStar <SS, DS> .AnytimeDStarState( ); this.aS.sv = sv; this.sm = sm; this.Ratio = Ratio; }
private static Metric g(DS ds, AnytimeDStarState aS) { Metric r; if (aS.G.TryGetValue(ds.ToString( ), out r)) { return(r); } else { return(Metric.PositiveInfinity); } }
private static SvenKey TopKey(AnytimeDStarState aS) { var _ = aS.OPEN.TopKey( ); if (_ == null) { _ = new SvenKey( ) { k1 = Metric.PositiveInfinity, k2 = Metric.PositiveInfinity }; } return(_); }
private static void MoveINCONSintoOPENAndResort(AnytimeDStarState aS) { var OldOPEN = aS.OPEN; aS.OPEN = new PriorityQueue <SvenKey, DS>( ); foreach (DS s in aS.INCONS) { aS.OPEN.Enqueue(key(s, aS), s); } while (!OldOPEN.IsEmpty( )) { DS s = OldOPEN.Dequeue( ); aS.OPEN.Enqueue(key(s, aS), s); } }
private static SvenKey key(DS s, AnytimeDStarState aS) { if (g(s, aS) > rhs(s, aS)) { return(new SvenKey( ) { k1 = rhs(s, aS) + aS.epsilon * aS.h(aS.Sstart, s), k2 = rhs(s, aS) }); } else { return(new SvenKey( ) { k1 = g(s, aS) + aS.h(aS.Sstart, s), k2 = g(s, aS) }); } }
private static Status ComputeorImprovePath(AnytimeDStarState aS) { while (TopKey(aS) < key(aS.Sstart, aS) || rhs(aS.Sstart, aS) != g(aS.Sstart, aS)) { aS.Expansions++; #if OpenGl if (aS.sv != null) { aS.sv.VisualizeAlg(aS.ss, aS.ss.InitialDynamicState, new OpenGLStateVisualizer.OpenGlVisulizationData( ) { G = aS.G, RHS = aS.RHS, U = aS.OPEN as PriorityQueue <SvenKey, GenericGridWorldDynamicState> }); } #endif if (aS.Expansions > aS.ComputationLimit) { return(Status.ComputationLimitExceded); } var s = aS.OPEN.Dequeue( ); if (g(s, aS) > rhs(s, aS)) { g(s, rhs(s, aS), aS); aS.CLOSED.Add(s); foreach (DS sp in Pred(s, aS)) { aS.Generations++; UpdateState(sp, aS); } } else { g(s, Metric.Infinity, aS); foreach (DS sp in PredUnionSelf(s, aS)) { aS.Generations++; UpdateState(sp, aS); } } } return(Status.NormalTermination); }
private static void UpdateState(DS s, AnytimeDStarState aS) { if (!s.Equals(aS.Sgoal)) { rhs(s, Min(sp => c(s, sp, aS) + g(sp, aS), Succ(s, aS)), aS); } if (aS.OPEN.Contains(s)) { aS.OPEN.Remove(s); } if (g(s, aS) != rhs(s, aS)) { if (!aS.CLOSED.Contains(s)) { aS.OPEN.Enqueue(key(s, aS), s); } else { aS.INCONS.Add(s); } } }
private static DS[] Succ(DS u, AnytimeDStarState aS) { return(u.Neighbors(aS.Ops, aS.ss, false).ToArray( )); }