From 2de4085c87aec14c6dec10f36664912958d34a2b Mon Sep 17 00:00:00 2001 From: xiaojin Date: Wed, 30 Jun 2021 20:50:39 +0800 Subject: [PATCH] =?UTF-8?q?=F0=9F=94=A7=20build:=20add=20htn?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- tools/Fluid-HTN/BaseDomainBuilder.cs | 355 +++++++++++++++++ tools/Fluid-HTN/Conditions/FuncCondition.cs | 37 ++ tools/Fluid-HTN/Conditions/ICondition.cs | 8 + tools/Fluid-HTN/Contexts/BaseContext.cs | 186 +++++++++ tools/Fluid-HTN/Contexts/IContext.cs | 108 ++++++ .../Fluid-HTN/Debug/DecompositionLogEntry.cs | 58 +++ tools/Fluid-HTN/Domain.cs | 238 ++++++++++++ tools/Fluid-HTN/DomainBuilder.cs | 24 ++ tools/Fluid-HTN/Effects/ActionEffect.cs | 38 ++ tools/Fluid-HTN/Effects/EffectType.cs | 9 + tools/Fluid-HTN/Effects/IEffect.cs | 9 + tools/Fluid-HTN/Factory/DefaultFactory.cs | 52 +++ tools/Fluid-HTN/Factory/IFactory.cs | 20 + tools/Fluid-HTN/Fluid-HTN.csproj | 80 ++++ tools/Fluid-HTN/Fluid.HTN.asmdef | 8 + tools/Fluid-HTN/IDomain.cs | 12 + tools/Fluid-HTN/Operators/FuncOperator.cs | 37 ++ tools/Fluid-HTN/Operators/IOperator.cs | 8 + tools/Fluid-HTN/Planners/Planner.cs | 357 ++++++++++++++++++ tools/Fluid-HTN/Properties/AssemblyInfo.cs | 35 ++ .../Tasks/CompoundTasks/CompoundTask.cs | 80 ++++ .../CompoundTasks/DecompositionStatus.cs | 10 + .../Tasks/CompoundTasks/ICompoundTask.cs | 18 + .../Tasks/CompoundTasks/IDecomposeAll.cs | 12 + .../Tasks/CompoundTasks/PausePlanTask.cs | 58 +++ .../Fluid-HTN/Tasks/CompoundTasks/Selector.cs | 253 +++++++++++++ .../Fluid-HTN/Tasks/CompoundTasks/Sequence.cs | 231 ++++++++++++ .../Fluid-HTN/Tasks/CompoundTasks/TaskRoot.cs | 8 + tools/Fluid-HTN/Tasks/ITask.cs | 45 +++ tools/Fluid-HTN/Tasks/OtherTasks/Slot.cs | 79 ++++ .../Tasks/PrimitiveTasks/IPrimitiveTask.cs | 31 ++ .../Tasks/PrimitiveTasks/PrimitiveTask.cs | 108 ++++++ tools/Fluid-HTN/Tasks/TaskStatus.cs | 9 + tools/Fluid-HTN/package.json | 9 + 34 files changed, 2630 insertions(+) create mode 100644 tools/Fluid-HTN/BaseDomainBuilder.cs create mode 100644 tools/Fluid-HTN/Conditions/FuncCondition.cs create mode 100644 tools/Fluid-HTN/Conditions/ICondition.cs create mode 100644 tools/Fluid-HTN/Contexts/BaseContext.cs create mode 100644 tools/Fluid-HTN/Contexts/IContext.cs create mode 100644 tools/Fluid-HTN/Debug/DecompositionLogEntry.cs create mode 100644 tools/Fluid-HTN/Domain.cs create mode 100644 tools/Fluid-HTN/DomainBuilder.cs create mode 100644 tools/Fluid-HTN/Effects/ActionEffect.cs create mode 100644 tools/Fluid-HTN/Effects/EffectType.cs create mode 100644 tools/Fluid-HTN/Effects/IEffect.cs create mode 100644 tools/Fluid-HTN/Factory/DefaultFactory.cs create mode 100644 tools/Fluid-HTN/Factory/IFactory.cs create mode 100644 tools/Fluid-HTN/Fluid-HTN.csproj create mode 100644 tools/Fluid-HTN/Fluid.HTN.asmdef create mode 100644 tools/Fluid-HTN/IDomain.cs create mode 100644 tools/Fluid-HTN/Operators/FuncOperator.cs create mode 100644 tools/Fluid-HTN/Operators/IOperator.cs create mode 100644 tools/Fluid-HTN/Planners/Planner.cs create mode 100644 tools/Fluid-HTN/Properties/AssemblyInfo.cs create mode 100644 tools/Fluid-HTN/Tasks/CompoundTasks/CompoundTask.cs create mode 100644 tools/Fluid-HTN/Tasks/CompoundTasks/DecompositionStatus.cs create mode 100644 tools/Fluid-HTN/Tasks/CompoundTasks/ICompoundTask.cs create mode 100644 tools/Fluid-HTN/Tasks/CompoundTasks/IDecomposeAll.cs create mode 100644 tools/Fluid-HTN/Tasks/CompoundTasks/PausePlanTask.cs create mode 100644 tools/Fluid-HTN/Tasks/CompoundTasks/Selector.cs create mode 100644 tools/Fluid-HTN/Tasks/CompoundTasks/Sequence.cs create mode 100644 tools/Fluid-HTN/Tasks/CompoundTasks/TaskRoot.cs create mode 100644 tools/Fluid-HTN/Tasks/ITask.cs create mode 100644 tools/Fluid-HTN/Tasks/OtherTasks/Slot.cs create mode 100644 tools/Fluid-HTN/Tasks/PrimitiveTasks/IPrimitiveTask.cs create mode 100644 tools/Fluid-HTN/Tasks/PrimitiveTasks/PrimitiveTask.cs create mode 100644 tools/Fluid-HTN/Tasks/TaskStatus.cs create mode 100644 tools/Fluid-HTN/package.json diff --git a/tools/Fluid-HTN/BaseDomainBuilder.cs b/tools/Fluid-HTN/BaseDomainBuilder.cs new file mode 100644 index 0000000..5126639 --- /dev/null +++ b/tools/Fluid-HTN/BaseDomainBuilder.cs @@ -0,0 +1,355 @@ +using System; +using System.Collections.Generic; +using FluidHTN.Compounds; +using FluidHTN.Conditions; +using FluidHTN.Effects; +using FluidHTN.Factory; +using FluidHTN.Operators; +using FluidHTN.PrimitiveTasks; + +namespace FluidHTN +{ + public abstract class BaseDomainBuilder + where DB : BaseDomainBuilder + where T : IContext + { + // ========================================================= FIELDS + + protected readonly Domain _domain; + protected List _pointers; + protected readonly IFactory _factory; + + // ========================================================= CONSTRUCTION + + public BaseDomainBuilder(string domainName, IFactory factory) + { + _factory = factory; + _domain = new Domain(domainName); + _pointers = _factory.CreateList(); + _pointers.Add(_domain.Root); + } + + // ========================================================= PROPERTIES + + public ITask Pointer + { + get + { + if (_pointers.Count == 0) return null; + return _pointers[_pointers.Count - 1]; + } + } + + // ========================================================= HIERARCHY HANDLING + + /// + /// Compound tasks are where HTN get their “hierarchical” nature. You can think of a compound task as + /// a high level task that has multiple ways of being accomplished. There are primarily two types of + /// compound tasks. Selectors and Sequencers. A Selector must be able to decompose a single sub-task, + /// while a Sequence must be able to decompose all its sub-tasks successfully for itself to have decomposed + /// successfully. There is nothing stopping you from extending this toolset with RandomSelect, UtilitySelect, + /// etc. These tasks are decomposed until we're left with only Primitive Tasks, which represent a final plan. + /// Compound tasks are comprised of a set of subtasks and a set of conditions. + /// http://www.gameaipro.com/GameAIPro/GameAIPro_Chapter12_Exploring_HTN_Planners_through_Example.pdf + /// + /// The type of compound task + /// The name given to the task, mainly for debug/display purposes + /// + public DB CompoundTask

(string name) where P : ICompoundTask, new() + { + var parent = new P(); + return CompoundTask(name, parent); + } + + ///

+ /// Compound tasks are where HTN get their “hierarchical” nature. You can think of a compound task as + /// a high level task that has multiple ways of being accomplished. There are primarily two types of + /// compound tasks. Selectors and Sequencers. A Selector must be able to decompose a single sub-task, + /// while a Sequence must be able to decompose all its sub-tasks successfully for itself to have decomposed + /// successfully. There is nothing stopping you from extending this toolset with RandomSelect, UtilitySelect, + /// etc. These tasks are decomposed until we're left with only Primitive Tasks, which represent a final plan. + /// Compound tasks are comprised of a set of subtasks and a set of conditions. + /// http://www.gameaipro.com/GameAIPro/GameAIPro_Chapter12_Exploring_HTN_Planners_through_Example.pdf + /// + /// The type of compound task + /// The name given to the task, mainly for debug/display purposes + /// The task instance + /// + public DB CompoundTask

(string name, P task) where P : ICompoundTask + { + if (task != null) + { + if (Pointer is ICompoundTask compoundTask) + { + task.Name = name; + _domain.Add(compoundTask, task); + _pointers.Add(task); + } + else + { + throw new Exception( + "Pointer is not a compound task type. Did you forget an End() after a Primitive Task Action was defined?"); + } + } + else + { + throw new ArgumentNullException( + "task"); + } + + return (DB) this; + } + + ///

+ /// Primitive tasks represent a single step that can be performed by our AI. A set of primitive tasks is + /// the plan that we are ultimately getting out of the HTN. Primitive tasks are comprised of an operator, + /// a set of effects, a set of conditions and a set of executing conditions. + /// http://www.gameaipro.com/GameAIPro/GameAIPro_Chapter12_Exploring_HTN_Planners_through_Example.pdf + /// + /// The type of primitive task + /// The name given to the task, mainly for debug/display purposes + /// + public DB PrimitiveTask

(string name) where P : IPrimitiveTask, new() + { + if (Pointer is ICompoundTask compoundTask) + { + var parent = new P { Name = name }; + _domain.Add(compoundTask, parent); + _pointers.Add(parent); + } + else + { + throw new Exception( + "Pointer is not a compound task type. Did you forget an End() after a Primitive Task Action was defined?"); + } + + return (DB) this; + } + + ///

+ /// Partial planning is one of the most powerful features of HTN. In simplest terms, it allows + /// the planner the ability to not fully decompose a complete plan. HTN is able to do this because + /// it uses forward decomposition or forward search to find plans. That is, the planner starts with + /// the current world state and plans forward in time from that. This allows the planner to only + /// plan ahead a few steps. + /// http://www.gameaipro.com/GameAIPro/GameAIPro_Chapter12_Exploring_HTN_Planners_through_Example.pdf + /// + /// + protected DB PausePlanTask() + { + if (Pointer is IDecomposeAll compoundTask) + { + var parent = new PausePlanTask() { Name = "Pause Plan" }; + _domain.Add(compoundTask, parent); + } + else + { + throw new Exception( + "Pointer is not a decompose-all compound task type, like a Sequence. Maybe you tried to Pause Plan a Selector, or forget an End() after a Primitive Task Action was defined?"); + } + + return (DB) this; + } + + // ========================================================= COMPOUND TASKS + + /// + /// A compound task that requires all sub-tasks to be valid. + /// Sub-tasks can be sequences, selectors or actions. + /// + /// + /// + public DB Sequence(string name) + { + return CompoundTask(name); + } + + /// + /// A compound task that requires a single sub-task to be valid. + /// Sub-tasks can be sequences, selectors or actions. + /// + /// + /// + public DB Select(string name) + { + return CompoundTask(name); + } + + // ========================================================= PRIMITIVE TASKS + + /// + /// A primitive task that can contain conditions, an operator and effects. + /// + /// + /// + public DB Action(string name) + { + return PrimitiveTask(name); + } + + // ========================================================= CONDITIONS + + /// + /// A precondition is a boolean statement required for the parent task to validate. + /// + /// + /// + /// + public DB Condition(string name, Func condition) + { + var cond = new FuncCondition(name, condition); + Pointer.AddCondition(cond); + + return (DB) this; + } + + /// + /// An executing condition is a boolean statement validated before every call to the current + /// primitive task's operator update tick. It's only supported inside primitive tasks / Actions. + /// Note that this condition is never validated during planning, only during execution. + /// + /// + /// + /// + public DB ExecutingCondition(string name, Func condition) + { + if (Pointer is IPrimitiveTask task) + { + var cond = new FuncCondition(name, condition); + task.AddExecutingCondition(cond); + } + else + { + throw new Exception("Tried to add an Executing Condition, but the Pointer is not a Primitive Task!"); + } + + return (DB) this; + } + + // ========================================================= OPERATORS + + /// + /// The operator of an Action / primitive task. + /// + /// + /// + public DB Do(Func action, Action forceStopAction = null) + { + if (Pointer is IPrimitiveTask task) + { + var op = new FuncOperator(action, forceStopAction); + task.SetOperator(op); + } + else + { + throw new Exception("Tried to add an Operator, but the Pointer is not a Primitive Task!"); + } + + return (DB) this; + } + + // ========================================================= EFFECTS + + /// + /// Effects can be added to an Action / primitive task. + /// + /// + /// + /// + /// + public DB Effect(string name, EffectType effectType, Action action) + { + if (Pointer is IPrimitiveTask task) + { + var effect = new ActionEffect(name, effectType, action); + task.AddEffect(effect); + } + else + { + throw new Exception("Tried to add an Effect, but the Pointer is not a Primitive Task!"); + } + + return (DB) this; + } + + // ========================================================= OTHER OPERANDS + + /// + /// Every task encapsulation must end with a call to End(), otherwise subsequent calls will be applied wrong. + /// + /// + public DB End() + { + _pointers.RemoveAt(_pointers.Count - 1); + return (DB) this; + } + + /// + /// We can splice multiple domains together, allowing us to define reusable sub-domains. + /// + /// + /// + public DB Splice(Domain domain) + { + if (Pointer is ICompoundTask compoundTask) + _domain.Add(compoundTask, domain.Root); + else + throw new Exception( + "Pointer is not a compound task type. Did you forget an End()?"); + + return (DB) this; + } + + /// + /// The identifier associated with a slot can be used to splice + /// sub-domains onto the domain, and remove them, at runtime. + /// Use TrySetSlotDomain and ClearSlot on the domain instance at + /// runtime to manage this feature. SlotId can typically be implemented + /// as an enum. + /// + public DB Slot(int slotId) + { + if (Pointer is ICompoundTask compoundTask) + { + var slot = new Slot() { SlotId = slotId, Name = $"Slot {slotId}" }; + _domain.Add(compoundTask, slot); + } + else + throw new Exception( + "Pointer is not a compound task type. Did you forget an End()?"); + + return (DB) this; + } + + /// + /// We can add a Pause Plan when in a sequence in our domain definition, + /// and this will give us partial planning. + /// It means that we can tell our planner to only plan up to a certain point, + /// then stop. If the partial plan completes execution successfully, the next + /// time we try to find a plan, we will continue planning where we left off. + /// Typical use cases is to split after we navigate toward a location, since + /// this is often time consuming, it's hard to predict the world state when + /// we have reached the destination, and thus there's little point wasting + /// milliseconds on planning further into the future at that point. We might + /// still want to plan what to do when reaching the destination, however, and + /// this is where partial plans come into play. + /// + public DB PausePlan() + { + return PausePlanTask(); + } + + /// + /// Build the designed domain and return a domain instance. + /// + /// + public Domain Build() + { + if (Pointer != _domain.Root) + throw new Exception($"The domain definition lacks one or more End() statements. Pointer is '{Pointer.Name}', but expected '{_domain.Root.Name}'."); + + _factory.FreeList(ref _pointers); + return _domain; + } + } +} diff --git a/tools/Fluid-HTN/Conditions/FuncCondition.cs b/tools/Fluid-HTN/Conditions/FuncCondition.cs new file mode 100644 index 0000000..f9ec18d --- /dev/null +++ b/tools/Fluid-HTN/Conditions/FuncCondition.cs @@ -0,0 +1,37 @@ +using System; + +namespace FluidHTN.Conditions +{ + public class FuncCondition : ICondition where T : IContext + { + // ========================================================= FIELDS + + private readonly Func _func; + + // ========================================================= CONSTRUCTION + + public FuncCondition(string name, Func func) + { + Name = name; + _func = func; + } + + // ========================================================= PROPERTIES + + public string Name { get; } + + // ========================================================= VALIDITY + + public bool IsValid(IContext ctx) + { + if (ctx is T c) + { + var result = _func?.Invoke(c) ?? false; + if (ctx.LogDecomposition) ctx.Log(Name, $"FuncCondition.IsValid:{result}", ctx.CurrentDecompositionDepth+1, this, result ? ConsoleColor.DarkGreen : ConsoleColor.DarkRed); + return result; + } + + throw new Exception("Unexpected context type!"); + } + } +} diff --git a/tools/Fluid-HTN/Conditions/ICondition.cs b/tools/Fluid-HTN/Conditions/ICondition.cs new file mode 100644 index 0000000..5a32f43 --- /dev/null +++ b/tools/Fluid-HTN/Conditions/ICondition.cs @@ -0,0 +1,8 @@ +namespace FluidHTN.Conditions +{ + public interface ICondition + { + string Name { get; } + bool IsValid(IContext ctx); + } +} diff --git a/tools/Fluid-HTN/Contexts/BaseContext.cs b/tools/Fluid-HTN/Contexts/BaseContext.cs new file mode 100644 index 0000000..8a92ec9 --- /dev/null +++ b/tools/Fluid-HTN/Contexts/BaseContext.cs @@ -0,0 +1,186 @@ +using System; +using System.Collections.Generic; +using FluidHTN.Conditions; +using FluidHTN.Debug; +using FluidHTN.Factory; + +namespace FluidHTN.Contexts +{ + public abstract class BaseContext : IContext + { + // ========================================================= PROPERTIES + + public bool IsInitialized { get; protected set; } = false; + public bool IsDirty { get; set; } + public ContextState ContextState { get; set; } = ContextState.Executing; + public int CurrentDecompositionDepth { get; set; } = 0; + public abstract IFactory Factory { get; set; } + public List MethodTraversalRecord { get; set; } = new List(); + public List LastMTR { get; } = new List(); + public abstract List MTRDebug { get; set; } + public abstract List LastMTRDebug { get; set; } + public abstract bool DebugMTR { get; } + public abstract Queue DecompositionLog { get; set; } + public abstract bool LogDecomposition { get; } + public Queue PartialPlanQueue { get; set; } = new Queue(); + public bool HasPausedPartialPlan { get; set; } = false; + + public abstract byte[] WorldState { get; } + + public Stack>[] WorldStateChangeStack { get; protected set; } + + // ========================================================= INITIALIZATION + + public virtual void Init() + { + if (WorldStateChangeStack == null) + { + WorldStateChangeStack = new Stack>[WorldState.Length]; + for (var i = 0; i < WorldState.Length; i++) + WorldStateChangeStack[i] = new Stack>(); + } + + if (DebugMTR) + { + if (MTRDebug == null) MTRDebug = new List(); + if (LastMTRDebug == null) LastMTRDebug = new List(); + } + + if (LogDecomposition) + { + if (DecompositionLog == null) DecompositionLog = new Queue(); + } + + IsInitialized = true; + } + + // ========================================================= STATE HANDLING + + public bool HasState(int state, byte value) + { + return GetState(state) == value; + } + + public byte GetState(int state) + { + if (ContextState == ContextState.Executing) return WorldState[state]; + + if (WorldStateChangeStack[state].Count == 0) return WorldState[state]; + + return WorldStateChangeStack[state].Peek().Value; + } + + public virtual void SetState(int state, byte value, bool setAsDirty = true, EffectType e = EffectType.Permanent) + { + if (ContextState == ContextState.Executing) + { + // Prevent setting the world state dirty if we're not changing anything. + if (WorldState[state] == value) + return; + + WorldState[state] = value; + if (setAsDirty) + IsDirty = true; // When a state change during execution, we need to mark the context dirty for replanning! + } + else + { + WorldStateChangeStack[state].Push(new KeyValuePair(e, value)); + } + } + + // ========================================================= STATE STACK HANDLING + + public int[] GetWorldStateChangeDepth(IFactory factory) + { + var stackDepth = factory.CreateArray(WorldStateChangeStack.Length); + for (var i = 0; i < WorldStateChangeStack.Length; i++) stackDepth[i] = WorldStateChangeStack[i]?.Count ?? 0; + + return stackDepth; + } + + public void TrimForExecution() + { + if (ContextState == ContextState.Executing) + throw new Exception("Can not trim a context when in execution mode"); + + foreach (var stack in WorldStateChangeStack) + while (stack.Count != 0 && stack.Peek().Key != EffectType.Permanent) + stack.Pop(); + } + + public void TrimToStackDepth(int[] stackDepth) + { + if (ContextState == ContextState.Executing) + throw new Exception("Can not trim a context when in execution mode"); + + for (var i = 0; i < stackDepth.Length; i++) + { + var stack = WorldStateChangeStack[i]; + while (stack.Count > stackDepth[i]) stack.Pop(); + } + } + + // ========================================================= STATE RESET + + public virtual void Reset() + { + MethodTraversalRecord?.Clear(); + LastMTR?.Clear(); + + if (DebugMTR) + { + MTRDebug?.Clear(); + LastMTRDebug?.Clear(); + } + + IsInitialized = false; + } + + // ========================================================= DECOMPOSITION LOGGING + + public void Log(string name, string description, int depth, ITask task, ConsoleColor color = ConsoleColor.White) + { + if (LogDecomposition == false) + return; + + DecompositionLog.Enqueue(new DecomposedCompoundTaskEntry + { + Name = name, + Description = description, + Entry = task, + Depth = depth, + Color = color, + }); + } + + public void Log(string name, string description, int depth, ICondition condition, ConsoleColor color = ConsoleColor.DarkGreen) + { + if (LogDecomposition == false) + return; + + DecompositionLog.Enqueue(new DecomposedConditionEntry + { + Name = name, + Description = description, + Entry = condition, + Depth = depth, + Color = color + }); + } + + public void Log(string name, string description, int depth, IEffect effect, ConsoleColor color = ConsoleColor.DarkYellow) + { + if (LogDecomposition == false) + return; + + DecompositionLog.Enqueue(new DecomposedEffectEntry + { + Name = name, + Description = description, + Entry = effect, + Depth = depth, + Color = color, + }); + } + } +} diff --git a/tools/Fluid-HTN/Contexts/IContext.cs b/tools/Fluid-HTN/Contexts/IContext.cs new file mode 100644 index 0000000..308a68f --- /dev/null +++ b/tools/Fluid-HTN/Contexts/IContext.cs @@ -0,0 +1,108 @@ +using System; +using System.Collections.Generic; +using FluidHTN.Compounds; +using FluidHTN.Conditions; +using FluidHTN.Debug; +using FluidHTN.Factory; + +namespace FluidHTN +{ + /// + /// The state our context can be in. This is essentially planning or execution state. + /// + public enum ContextState + { + Planning, + Executing + } + + public struct PartialPlanEntry + { + public ICompoundTask Task; + public int TaskIndex; + } + + public interface IContext + { + bool IsInitialized { get; } + bool IsDirty { get; set; } + ContextState ContextState { get; set; } + int CurrentDecompositionDepth { get; set; } + + IFactory Factory { get; set; } + + /// + /// The Method Traversal Record is used while decomposing a domain and + /// records the valid decomposition indices as we go through our + /// decomposition process. + /// It "should" be enough to only record decomposition traversal in Selectors. + /// This can be used to compare LastMTR with the MTR, and reject + /// a new plan early if it is of lower priority than the last plan. + /// It is the user's responsibility to set the instance of the MTR, so that + /// the user is free to use pooled instances, or whatever optimization they + /// see fit. + /// + List MethodTraversalRecord { get; set; } + + List MTRDebug { get; set; } + + /// + /// The Method Traversal Record that was recorded for the currently + /// running plan. + /// If a plan completes successfully, this should be cleared. + /// It is the user's responsibility to set the instance of the MTR, so that + /// the user is free to use pooled instances, or whatever optimization they + /// see fit. + /// + List LastMTR { get; } + + List LastMTRDebug { get; set; } + + /// + /// Whether the planning system should collect debug information about our Method Traversal Record. + /// + bool DebugMTR { get; } + + /// + /// + Queue DecompositionLog { get; set; } + + /// + /// Whether our planning system should log our decomposition. Specially condition success vs failure. + /// + bool LogDecomposition { get; } + + /// + /// + /// + Queue PartialPlanQueue { get; set; } + + bool HasPausedPartialPlan { get; set; } + + byte[] WorldState { get; } + + /// + /// A stack of changes applied to each world state entry during planning. + /// This is necessary if one wants to support planner-only and plan&execute effects. + /// + Stack>[] WorldStateChangeStack { get; } + + /// + /// Reset the context state to default values. + /// + void Reset(); + + void TrimForExecution(); + void TrimToStackDepth(int[] stackDepth); + + bool HasState(int state, byte value); + byte GetState(int state); + void SetState(int state, byte value, bool setAsDirty = true, EffectType e = EffectType.Permanent); + + int[] GetWorldStateChangeDepth(IFactory factory); + + void Log(string name, string description, int depth, ITask task, ConsoleColor color = ConsoleColor.White); + void Log(string name, string description, int depth, ICondition condition, ConsoleColor color = ConsoleColor.DarkGreen); + void Log(string name, string description, int depth, IEffect effect, ConsoleColor color = ConsoleColor.DarkYellow); + } +} diff --git a/tools/Fluid-HTN/Debug/DecompositionLogEntry.cs b/tools/Fluid-HTN/Debug/DecompositionLogEntry.cs new file mode 100644 index 0000000..1ca77a5 --- /dev/null +++ b/tools/Fluid-HTN/Debug/DecompositionLogEntry.cs @@ -0,0 +1,58 @@ +using System; +using FluidHTN.Conditions; + +namespace FluidHTN.Debug +{ + public static class Debug + { + public static string DepthToString(int depth) + { + string s = ""; + for (var i = 0; i < depth; i++) + { + s += "\t"; + } + + s += "- "; + return s; + } + } + public interface IBaseDecompositionLogEntry + { + string Name { get; set; } + string Description { get; set; } + int Depth { get; set; } + ConsoleColor Color { get; set; } + string ToString(); + } + + public interface IDecompositionLogEntry : IBaseDecompositionLogEntry + { + T Entry { get; set; } + } + + public struct DecomposedCompoundTaskEntry : IDecompositionLogEntry + { + public string Name { get; set; } + public string Description { get; set; } + public int Depth { get; set; } + public ConsoleColor Color { get; set; } + public ITask Entry { get; set; } + } + + public struct DecomposedConditionEntry : IDecompositionLogEntry { + public string Name { get; set; } + public string Description { get; set; } + public int Depth { get; set; } + public ConsoleColor Color { get; set; } + public ICondition Entry { get; set; } + } + + public struct DecomposedEffectEntry : IDecompositionLogEntry { + public string Name { get; set; } + public string Description { get; set; } + public int Depth { get; set; } + public ConsoleColor Color { get; set; } + public IEffect Entry { get; set; } + } +} diff --git a/tools/Fluid-HTN/Domain.cs b/tools/Fluid-HTN/Domain.cs new file mode 100644 index 0000000..7a52b53 --- /dev/null +++ b/tools/Fluid-HTN/Domain.cs @@ -0,0 +1,238 @@ +using System; +using System.Collections.Generic; +using FluidHTN.Compounds; + +namespace FluidHTN +{ + public class Domain : IDomain where T : IContext + { + // ========================================================= FIELDS + + private Dictionary _slots = null; + + // ========================================================= CONSTRUCTION + + public Domain(string name) + { + Root = new TaskRoot { Name = name, Parent = null }; + } + // ========================================================= PROPERTIES + + public TaskRoot Root { get; } + + // ========================================================= HIERARCHY HANDLING + + public void Add(ICompoundTask parent, ITask subtask) + { + if (parent == subtask) + throw new Exception("Parent-task and Sub-task can't be the same instance!"); + + parent.AddSubtask(subtask); + subtask.Parent = parent; + } + + public void Add(ICompoundTask parent, Slot slot) + { + if (parent == slot) + throw new Exception("Parent-task and Sub-task can't be the same instance!"); + + if (_slots != null) + { + if (_slots.ContainsKey(slot.SlotId)) + { + throw new Exception("This slot id already exist in the domain definition!"); + } + } + + parent.AddSubtask(slot); + slot.Parent = parent; + + if(_slots == null) + { + _slots = new Dictionary(); + } + + _slots.Add(slot.SlotId, slot); + } + + // ========================================================= PLANNING + + public DecompositionStatus FindPlan(T ctx, out Queue plan) + { + if (ctx.IsInitialized == false) + throw new Exception("Context was not initialized!"); + + if (ctx.MethodTraversalRecord == null) + throw new Exception("We require the Method Traversal Record to have a valid instance."); + + ctx.ContextState = ContextState.Planning; + + plan = null; + var status = DecompositionStatus.Rejected; + + // We first check whether we have a stored start task. This is true + // if we had a partial plan pause somewhere in our plan, and we now + // want to continue where we left off. + // If this is the case, we don't erase the MTR, but continue building it. + // However, if we have a partial plan, but LastMTR is not 0, that means + // that the partial plan is still running, but something triggered a replan. + // When this happens, we have to plan from the domain root (we're not + // continuing the current plan), so that we're open for other plans to replace + // the running partial plan. + if (ctx.HasPausedPartialPlan && ctx.LastMTR.Count == 0) + { + ctx.HasPausedPartialPlan = false; + while (ctx.PartialPlanQueue.Count > 0) + { + var kvp = ctx.PartialPlanQueue.Dequeue(); + if (plan == null) + { + status = kvp.Task.Decompose(ctx, kvp.TaskIndex, out plan); + } + else + { + status = kvp.Task.Decompose(ctx, kvp.TaskIndex, out var p); + if (status == DecompositionStatus.Succeeded || status == DecompositionStatus.Partial) + { + while (p.Count > 0) + { + plan.Enqueue(p.Dequeue()); + } + } + } + + // While continuing a partial plan, we might encounter + // a new pause. + if (ctx.HasPausedPartialPlan) + break; + } + + // If we failed to continue the paused partial plan, + // then we have to start planning from the root. + if (status == DecompositionStatus.Rejected || status == DecompositionStatus.Failed) + { + ctx.MethodTraversalRecord.Clear(); + if (ctx.DebugMTR) ctx.MTRDebug.Clear(); + + status = Root.Decompose(ctx, 0, out plan); + } + } + else + { + Queue lastPartialPlanQueue = null; + if (ctx.HasPausedPartialPlan) + { + ctx.HasPausedPartialPlan = false; + lastPartialPlanQueue = ctx.Factory.CreateQueue(); + while (ctx.PartialPlanQueue.Count > 0) + { + lastPartialPlanQueue.Enqueue(ctx.PartialPlanQueue.Dequeue()); + } + } + + // We only erase the MTR if we start from the root task of the domain. + ctx.MethodTraversalRecord.Clear(); + if (ctx.DebugMTR) ctx.MTRDebug.Clear(); + + status = Root.Decompose(ctx, 0, out plan); + + // If we failed to find a new plan, we have to restore the old plan, + // if it was a partial plan. + if (lastPartialPlanQueue != null) + { + if (status == DecompositionStatus.Rejected || status == DecompositionStatus.Failed) + { + ctx.HasPausedPartialPlan = true; + ctx.PartialPlanQueue.Clear(); + while (lastPartialPlanQueue.Count > 0) + { + ctx.PartialPlanQueue.Enqueue(lastPartialPlanQueue.Dequeue()); + } + ctx.Factory.FreeQueue(ref lastPartialPlanQueue); + } + } + } + + // If this MTR equals the last MTR, then we need to double check whether we ended up + // just finding the exact same plan. During decomposition each compound task can't check + // for equality, only for less than, so this case needs to be treated after the fact. + var isMTRsEqual = ctx.MethodTraversalRecord.Count == ctx.LastMTR.Count; + if (isMTRsEqual) + { + for (var i = 0; i < ctx.MethodTraversalRecord.Count; i++) + if (ctx.MethodTraversalRecord[i] < ctx.LastMTR[i]) + { + isMTRsEqual = false; + break; + } + + if (isMTRsEqual) + { + plan = null; + status = DecompositionStatus.Rejected; + } + } + + if (status == DecompositionStatus.Succeeded || status == DecompositionStatus.Partial) + { + // Trim away any plan-only or plan&execute effects from the world state change stack, that only + // permanent effects on the world state remains now that the planning is done. + ctx.TrimForExecution(); + + // Apply permanent world state changes to the actual world state used during plan execution. + for (var i = 0; i < ctx.WorldStateChangeStack.Length; i++) + { + var stack = ctx.WorldStateChangeStack[i]; + if (stack != null && stack.Count > 0) + { + ctx.WorldState[i] = stack.Peek().Value; + stack.Clear(); + } + } + } + else + { + // Clear away any changes that might have been applied to the stack + // No changes should be made or tracked further when the plan failed. + for (var i = 0; i < ctx.WorldStateChangeStack.Length; i++) + { + var stack = ctx.WorldStateChangeStack[i]; + if (stack != null && stack.Count > 0) stack.Clear(); + } + } + + ctx.ContextState = ContextState.Executing; + return status; + } + + // ========================================================= SLOTS + + /// + /// At runtime, set a sub-domain to the slot with the given id. + /// This can be used with Smart Objects, to extend the behavior + /// of an agent at runtime. + /// + public bool TrySetSlotDomain(int slotId, Domain subDomain) + { + if(_slots != null && _slots.TryGetValue(slotId, out var slot)) + { + return slot.Set(subDomain.Root); + } + + return false; + } + + /// + /// At runtime, clear the sub-domain from the slot with the given id. + /// This can be used with Smart Objects, to extend the behavior + /// of an agent at runtime. + /// + public void ClearSlot(int slotId) + { + if (_slots != null && _slots.TryGetValue(slotId, out var slot)) + { + slot.Clear(); + } + } + } +} diff --git a/tools/Fluid-HTN/DomainBuilder.cs b/tools/Fluid-HTN/DomainBuilder.cs new file mode 100644 index 0000000..dfc7dec --- /dev/null +++ b/tools/Fluid-HTN/DomainBuilder.cs @@ -0,0 +1,24 @@ +using FluidHTN.Factory; + +namespace FluidHTN +{ + /// + /// A simple domain builder for easy use when one just need the core functionality + /// of the BaseDomainBuilder. This class is sealed, so if you want to extend the + /// functionality of the domain builder, extend BaseDomainBuilder instead. + /// + /// + public sealed class DomainBuilder : BaseDomainBuilder, T> + where T : IContext + { + // ========================================================= CONSTRUCTION + + public DomainBuilder(string domainName) : base(domainName, new DefaultFactory()) + { + } + + public DomainBuilder(string domainName, IFactory factory) : base(domainName, factory) + { + } + } +} \ No newline at end of file diff --git a/tools/Fluid-HTN/Effects/ActionEffect.cs b/tools/Fluid-HTN/Effects/ActionEffect.cs new file mode 100644 index 0000000..ea945f8 --- /dev/null +++ b/tools/Fluid-HTN/Effects/ActionEffect.cs @@ -0,0 +1,38 @@ +using System; + +namespace FluidHTN.Effects +{ + public class ActionEffect : IEffect where T : IContext + { + // ========================================================= FIELDS + + private readonly Action _action; + + // ========================================================= CONSTRUCTION + + public ActionEffect(string name, EffectType type, Action action) + { + Name = name; + Type = type; + _action = action; + } + + // ========================================================= PROPERTIES + + public string Name { get; } + public EffectType Type { get; } + + // ========================================================= FUNCTIONALITY + + public void Apply(IContext ctx) + { + if (ctx is T c) + { + if (ctx.LogDecomposition) ctx.Log(Name, $"ActionEffect.Apply:{Type}", ctx.CurrentDecompositionDepth+1, this); + _action?.Invoke(c, Type); + } + else + throw new Exception("Unexpected context type!"); + } + } +} diff --git a/tools/Fluid-HTN/Effects/EffectType.cs b/tools/Fluid-HTN/Effects/EffectType.cs new file mode 100644 index 0000000..53e075a --- /dev/null +++ b/tools/Fluid-HTN/Effects/EffectType.cs @@ -0,0 +1,9 @@ +namespace FluidHTN +{ + public enum EffectType + { + PlanAndExecute, + PlanOnly, + Permanent + } +} \ No newline at end of file diff --git a/tools/Fluid-HTN/Effects/IEffect.cs b/tools/Fluid-HTN/Effects/IEffect.cs new file mode 100644 index 0000000..24b0034 --- /dev/null +++ b/tools/Fluid-HTN/Effects/IEffect.cs @@ -0,0 +1,9 @@ +namespace FluidHTN +{ + public interface IEffect + { + string Name { get; } + EffectType Type { get; } + void Apply(IContext ctx); + } +} diff --git a/tools/Fluid-HTN/Factory/DefaultFactory.cs b/tools/Fluid-HTN/Factory/DefaultFactory.cs new file mode 100644 index 0000000..3494abe --- /dev/null +++ b/tools/Fluid-HTN/Factory/DefaultFactory.cs @@ -0,0 +1,52 @@ + +using System.Collections.Generic; + +namespace FluidHTN.Factory +{ + public sealed class DefaultFactory : IFactory + { + public T[] CreateArray(int length) + { + return new T[length]; + } + + public List CreateList() + { + return new List(); + } + + public Queue CreateQueue() + { + return new Queue(); + } + + public bool FreeArray(ref T[] array) + { + array = null; + return array == null; + } + + public bool FreeList(ref List list) + { + list = null; + return list == null; + } + + public bool FreeQueue(ref Queue queue) + { + queue = null; + return queue == null; + } + + public T Create() where T : new() + { + return new T(); + } + + public bool Free(ref T obj) + { + obj = default(T); + return obj == null; + } + } +} diff --git a/tools/Fluid-HTN/Factory/IFactory.cs b/tools/Fluid-HTN/Factory/IFactory.cs new file mode 100644 index 0000000..05a6469 --- /dev/null +++ b/tools/Fluid-HTN/Factory/IFactory.cs @@ -0,0 +1,20 @@ + +using System.Collections.Generic; + +namespace FluidHTN.Factory +{ + public interface IFactory + { + T[] CreateArray(int length); + bool FreeArray(ref T[] array); + + Queue CreateQueue(); + bool FreeQueue(ref Queue queue); + + List CreateList(); + bool FreeList(ref List list); + + T Create() where T : new(); + bool Free(ref T obj); + } +} diff --git a/tools/Fluid-HTN/Fluid-HTN.csproj b/tools/Fluid-HTN/Fluid-HTN.csproj new file mode 100644 index 0000000..d07e064 --- /dev/null +++ b/tools/Fluid-HTN/Fluid-HTN.csproj @@ -0,0 +1,80 @@ + + + + + Debug + AnyCPU + {B6908CED-5C0B-415C-9564-85F66A8B5025} + Library + Properties + FluidHTN + Fluid-HTN + v4.7.2 + 512 + true + + + + true + full + false + bin\Debug\ + DEBUG;TRACE + prompt + 4 + 7.1 + + + pdbonly + true + bin\Release\ + TRACE + prompt + 4 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/tools/Fluid-HTN/Fluid.HTN.asmdef b/tools/Fluid-HTN/Fluid.HTN.asmdef new file mode 100644 index 0000000..fd7894a --- /dev/null +++ b/tools/Fluid-HTN/Fluid.HTN.asmdef @@ -0,0 +1,8 @@ +{ + "name": "Fluid.HTN", + "references": [], + "optionalUnityReferences": [], + "includePlatforms": [], + "excludePlatforms": [], + "allowUnsafeCode": false +} diff --git a/tools/Fluid-HTN/IDomain.cs b/tools/Fluid-HTN/IDomain.cs new file mode 100644 index 0000000..ba15b7a --- /dev/null +++ b/tools/Fluid-HTN/IDomain.cs @@ -0,0 +1,12 @@ +using System.Collections.Generic; +using FluidHTN.Compounds; + +namespace FluidHTN +{ + public interface IDomain + { + TaskRoot Root { get; } + void Add(ICompoundTask parent, ITask subtask); + void Add(ICompoundTask parent, Slot slot); + } +} diff --git a/tools/Fluid-HTN/Operators/FuncOperator.cs b/tools/Fluid-HTN/Operators/FuncOperator.cs new file mode 100644 index 0000000..d894f25 --- /dev/null +++ b/tools/Fluid-HTN/Operators/FuncOperator.cs @@ -0,0 +1,37 @@ +using System; + +namespace FluidHTN.Operators +{ + public class FuncOperator : IOperator where T : IContext + { + // ========================================================= FIELDS + + private readonly Func _func; + private readonly Action _funcStop; + + // ========================================================= CONSTRUCTION + + public FuncOperator(Func func, Action funcStop = null) + { + _func = func; + _funcStop = funcStop; + } + + // ========================================================= FUNCTIONALITY + + public TaskStatus Update(IContext ctx) + { + if (ctx is T c) + return _func?.Invoke(c) ?? TaskStatus.Failure; + throw new Exception("Unexpected context type!"); + } + + public void Stop(IContext ctx) + { + if (ctx is T c) + _funcStop?.Invoke(c); + else + throw new Exception("Unexpected context type!"); + } + } +} \ No newline at end of file diff --git a/tools/Fluid-HTN/Operators/IOperator.cs b/tools/Fluid-HTN/Operators/IOperator.cs new file mode 100644 index 0000000..43b8df1 --- /dev/null +++ b/tools/Fluid-HTN/Operators/IOperator.cs @@ -0,0 +1,8 @@ +namespace FluidHTN.Operators +{ + public interface IOperator + { + TaskStatus Update(IContext ctx); + void Stop(IContext ctx); + } +} \ No newline at end of file diff --git a/tools/Fluid-HTN/Planners/Planner.cs b/tools/Fluid-HTN/Planners/Planner.cs new file mode 100644 index 0000000..a48f3b8 --- /dev/null +++ b/tools/Fluid-HTN/Planners/Planner.cs @@ -0,0 +1,357 @@ +using System; +using System.Collections.Generic; +using FluidHTN.Compounds; +using FluidHTN.Conditions; +using FluidHTN.PrimitiveTasks; + +namespace FluidHTN +{ + /// + /// A planner is a responsible for handling the management of finding plans in a domain, replan when the state of the + /// running plan + /// demands it, or look for a new potential plan if the world state gets dirty. + /// + /// + public class Planner where T : IContext + { + // ========================================================= FIELDS + + private ITask _currentTask; + private readonly Queue _plan = new Queue(); + + // ========================================================= FIELDS + public TaskStatus LastStatus { get; protected set; } + + // ========================================================= CALLBACKS + + /// + /// OnNewPlan(newPlan) is called when we found a new plan, and there is no + /// old plan to replace. + /// + public Action> OnNewPlan = null; + + /// + /// OnReplacePlan(oldPlan, currentTask, newPlan) is called when we're about to replace the + /// current plan with a new plan. + /// + public Action, ITask, Queue> OnReplacePlan = null; + + /// + /// OnNewTask(task) is called after we popped a new task off the current plan. + /// + public Action OnNewTask = null; + + /// + /// OnNewTaskConditionFailed(task, failedCondition) is called when we failed to + /// validate a condition on a new task. + /// + public Action OnNewTaskConditionFailed = null; + + /// + /// OnStopCurrentTask(task) is called when the currently running task was stopped + /// forcefully. + /// + public Action OnStopCurrentTask = null; + + /// + /// OnCurrentTaskCompletedSuccessfully(task) is called when the currently running task + /// completes successfully, and before its effects are applied. + /// + public Action OnCurrentTaskCompletedSuccessfully = null; + + /// + /// OnApplyEffect(effect) is called for each effect of the type PlanAndExecute on a + /// completed task. + /// + public Action OnApplyEffect = null; + + /// + /// OnCurrentTaskFailed(task) is called when the currently running task fails to complete. + /// + public Action OnCurrentTaskFailed = null; + + /// + /// OnCurrentTaskContinues(task) is called every tick that a currently running task + /// needs to continue. + /// + public Action OnCurrentTaskContinues = null; + + /// + /// OnCurrentTaskExecutingConditionFailed(task, condition) is called if an Executing Condition + /// fails. The Executing Conditions are checked before every call to task.Operator.Update(...). + /// + public Action OnCurrentTaskExecutingConditionFailed = null; + + // ========================================================= TICK PLAN + + /// + /// Call this with a domain and context instance to have the planner manage plan and task handling for the domain at + /// runtime. + /// If the plan completes or fails, the planner will find a new plan, or if the context is marked dirty, the planner + /// will attempt + /// a replan to see whether we can find a better plan now that the state of the world has changed. + /// This planner can also be used as a blueprint for writing a custom planner. + /// + /// + /// + public void Tick(Domain domain, T ctx, bool allowImmediateReplan = true) + { + if (ctx.IsInitialized == false) + throw new Exception("Context was not initialized!"); + + DecompositionStatus decompositionStatus = DecompositionStatus.Failed; + bool isTryingToReplacePlan = false; + // Check whether state has changed or the current plan has finished running. + // and if so, try to find a new plan. + if (_currentTask == null && (_plan.Count == 0) || ctx.IsDirty) + { + Queue lastPartialPlanQueue = null; + + var worldStateDirtyReplan = ctx.IsDirty; + ctx.IsDirty = false; + + if (worldStateDirtyReplan) + { + // If we're simply re-evaluating whether to replace the current plan because + // some world state got dirt, then we do not intend to continue a partial plan + // right now, but rather see whether the world state changed to a degree where + // we should pursue a better plan. Thus, if this replan fails to find a better + // plan, we have to add back the partial plan temps cached above. + if (ctx.HasPausedPartialPlan) + { + ctx.HasPausedPartialPlan = false; + lastPartialPlanQueue = ctx.Factory.CreateQueue(); + while (ctx.PartialPlanQueue.Count > 0) + { + lastPartialPlanQueue.Enqueue(ctx.PartialPlanQueue.Dequeue()); + } + + // We also need to ensure that the last mtr is up to date with the on-going MTR of the partial plan, + // so that any new potential plan that is decomposing from the domain root has to beat the currently + // running partial plan. + ctx.LastMTR.Clear(); + foreach (var record in ctx.MethodTraversalRecord) ctx.LastMTR.Add(record); + + if (ctx.DebugMTR) + { + ctx.LastMTRDebug.Clear(); + foreach (var record in ctx.MTRDebug) ctx.LastMTRDebug.Add(record); + } + } + } + + decompositionStatus = domain.FindPlan(ctx, out var newPlan); + isTryingToReplacePlan = _plan.Count > 0; + if (decompositionStatus == DecompositionStatus.Succeeded || decompositionStatus == DecompositionStatus.Partial) + { + if (OnReplacePlan != null && (_plan.Count > 0 || _currentTask != null)) + { + OnReplacePlan.Invoke(_plan, _currentTask, newPlan); + } + else if (OnNewPlan != null && _plan.Count == 0) + { + OnNewPlan.Invoke(newPlan); + } + + _plan.Clear(); + while (newPlan.Count > 0) _plan.Enqueue(newPlan.Dequeue()); + + if (_currentTask != null && _currentTask is IPrimitiveTask t) + { + OnStopCurrentTask?.Invoke(t); + t.Stop(ctx); + _currentTask = null; + } + + // Copy the MTR into our LastMTR to represent the current plan's decomposition record + // that must be beat to replace the plan. + if (ctx.MethodTraversalRecord != null) + { + ctx.LastMTR.Clear(); + foreach (var record in ctx.MethodTraversalRecord) ctx.LastMTR.Add(record); + + if (ctx.DebugMTR) + { + ctx.LastMTRDebug.Clear(); + foreach (var record in ctx.MTRDebug) ctx.LastMTRDebug.Add(record); + } + } + } + else if (lastPartialPlanQueue != null) + { + ctx.HasPausedPartialPlan = true; + ctx.PartialPlanQueue.Clear(); + while (lastPartialPlanQueue.Count > 0) + { + ctx.PartialPlanQueue.Enqueue(lastPartialPlanQueue.Dequeue()); + } + ctx.Factory.FreeQueue(ref lastPartialPlanQueue); + + if (ctx.LastMTR.Count > 0) + { + ctx.MethodTraversalRecord.Clear(); + foreach (var record in ctx.LastMTR) ctx.MethodTraversalRecord.Add(record); + ctx.LastMTR.Clear(); + + if (ctx.DebugMTR) + { + ctx.MTRDebug.Clear(); + foreach (var record in ctx.LastMTRDebug) ctx.MTRDebug.Add(record); + ctx.LastMTRDebug.Clear(); + } + } + } + } + + if (_currentTask == null && _plan.Count > 0) + { + _currentTask = _plan.Dequeue(); + if (_currentTask != null) + { + OnNewTask?.Invoke(_currentTask); + foreach (var condition in _currentTask.Conditions) + // If a condition failed, then the plan failed to progress! A replan is required. + if (condition.IsValid(ctx) == false) + { + OnNewTaskConditionFailed?.Invoke(_currentTask, condition); + + _currentTask = null; + _plan.Clear(); + + ctx.LastMTR.Clear(); + if (ctx.DebugMTR) ctx.LastMTRDebug.Clear(); + + ctx.HasPausedPartialPlan = false; + ctx.PartialPlanQueue.Clear(); + ctx.IsDirty = false; + + return; + } + } + } + + if (_currentTask != null) + if (_currentTask is IPrimitiveTask task) + { + if (task.Operator != null) + { + foreach (var condition in task.ExecutingConditions) + // If a condition failed, then the plan failed to progress! A replan is required. + if (condition.IsValid(ctx) == false) + { + OnCurrentTaskExecutingConditionFailed?.Invoke(task, condition); + + _currentTask = null; + _plan.Clear(); + + ctx.LastMTR.Clear(); + if (ctx.DebugMTR) ctx.LastMTRDebug.Clear(); + + ctx.HasPausedPartialPlan = false; + ctx.PartialPlanQueue.Clear(); + ctx.IsDirty = false; + + return; + } + + LastStatus = task.Operator.Update(ctx); + + // If the operation finished successfully, we set task to null so that we dequeue the next task in the plan the following tick. + if (LastStatus == TaskStatus.Success) + { + OnCurrentTaskCompletedSuccessfully?.Invoke(task); + + // All effects that is a result of running this task should be applied when the task is a success. + foreach (var effect in task.Effects) + { + if (effect.Type == EffectType.PlanAndExecute) + { + OnApplyEffect?.Invoke(effect); + effect.Apply(ctx); + } + } + + _currentTask = null; + if (_plan.Count == 0) + { + ctx.LastMTR.Clear(); + if (ctx.DebugMTR) ctx.LastMTRDebug.Clear(); + + ctx.IsDirty = false; + + if (allowImmediateReplan) Tick(domain, ctx, allowImmediateReplan: false); + } + } + + // If the operation failed to finish, we need to fail the entire plan, so that we will replan the next tick. + else if (LastStatus == TaskStatus.Failure) + { + OnCurrentTaskFailed?.Invoke(task); + + _currentTask = null; + _plan.Clear(); + + ctx.LastMTR.Clear(); + if (ctx.DebugMTR) ctx.LastMTRDebug.Clear(); + + ctx.HasPausedPartialPlan = false; + ctx.PartialPlanQueue.Clear(); + ctx.IsDirty = false; + } + + // Otherwise the operation isn't done yet and need to continue. + else + { + OnCurrentTaskContinues?.Invoke(task); + } + } + else + { + // This should not really happen if a domain is set up properly. + _currentTask = null; + LastStatus = TaskStatus.Failure; + } + } + + if (_currentTask == null && _plan.Count == 0 && isTryingToReplacePlan == false && + (decompositionStatus == DecompositionStatus.Failed || + decompositionStatus == DecompositionStatus.Rejected)) + { + LastStatus = TaskStatus.Failure; + } + } + + // ========================================================= RESET + + public void Reset(IContext ctx) + { + _plan.Clear(); + + if (_currentTask != null && _currentTask is IPrimitiveTask task) + { + task.Stop(ctx); + } + _currentTask = null; + } + + // ========================================================= GETTERS + + /// + /// Get the current plan. This is not a copy of the running plan, so treat it as read-only. + /// + /// + public Queue GetPlan() + { + return _plan; + } + + /// + /// Get the current task. + /// + /// + public ITask GetCurrentTask() + { + return _currentTask; + } + } +} diff --git a/tools/Fluid-HTN/Properties/AssemblyInfo.cs b/tools/Fluid-HTN/Properties/AssemblyInfo.cs new file mode 100644 index 0000000..7731ff1 --- /dev/null +++ b/tools/Fluid-HTN/Properties/AssemblyInfo.cs @@ -0,0 +1,35 @@ +using System.Reflection; +using System.Runtime.InteropServices; + +// General Information about an assembly is controlled through the following +// set of attributes. Change these attribute values to modify the information +// associated with an assembly. +[assembly: AssemblyTitle("Fluid-HTN")] +[assembly: AssemblyDescription("")] +[assembly: AssemblyConfiguration("")] +[assembly: AssemblyCompany("")] +[assembly: AssemblyProduct("Fluid-HTN")] +[assembly: AssemblyCopyright("Copyright © 2019")] +[assembly: AssemblyTrademark("")] +[assembly: AssemblyCulture("")] + +// Setting ComVisible to false makes the types in this assembly not visible +// to COM components. If you need to access a type in this assembly from +// COM, set the ComVisible attribute to true on that type. +[assembly: ComVisible(false)] + +// The following GUID is for the ID of the typelib if this project is exposed to COM +[assembly: Guid("b6908ced-5c0b-415c-9564-85f66a8b5025")] + +// Version information for an assembly consists of the following four values: +// +// Major Version +// Minor Version +// Build Number +// Revision +// +// You can specify all the values or you can default the Build and Revision Numbers +// by using the '*' as shown below: +// [assembly: AssemblyVersion("1.0.*")] +[assembly: AssemblyVersion("1.0.0.0")] +[assembly: AssemblyFileVersion("1.0.0.0")] \ No newline at end of file diff --git a/tools/Fluid-HTN/Tasks/CompoundTasks/CompoundTask.cs b/tools/Fluid-HTN/Tasks/CompoundTasks/CompoundTask.cs new file mode 100644 index 0000000..7b2740c --- /dev/null +++ b/tools/Fluid-HTN/Tasks/CompoundTasks/CompoundTask.cs @@ -0,0 +1,80 @@ +using System; +using System.Collections.Generic; +using FluidHTN.Conditions; + +namespace FluidHTN.Compounds +{ + public abstract class CompoundTask : ICompoundTask + { + // ========================================================= PROPERTIES + + public string Name { get; set; } + public ICompoundTask Parent { get; set; } + public List Conditions { get; } = new List(); + public TaskStatus LastStatus { get; private set; } + public List Subtasks { get; } = new List(); + + // ========================================================= VALIDITY + + public virtual DecompositionStatus OnIsValidFailed(IContext ctx) + { + return DecompositionStatus.Failed; + } + + // ========================================================= ADDERS + + public ITask AddCondition(ICondition condition) + { + Conditions.Add(condition); + return this; + } + + public ICompoundTask AddSubtask(ITask subtask) + { + Subtasks.Add(subtask); + return this; + } + + // ========================================================= DECOMPOSITION + + public DecompositionStatus Decompose(IContext ctx, int startIndex, out Queue result) + { + if (ctx.LogDecomposition) ctx.CurrentDecompositionDepth++; + var status = OnDecompose(ctx, startIndex, out result); + if (ctx.LogDecomposition) ctx.CurrentDecompositionDepth--; + return status; + } + + protected abstract DecompositionStatus OnDecompose(IContext ctx, int startIndex, out Queue result); + + protected abstract DecompositionStatus OnDecomposeTask(IContext ctx, ITask task, int taskIndex, int[] oldStackDepth, out Queue result); + + protected abstract DecompositionStatus OnDecomposeCompoundTask(IContext ctx, ICompoundTask task, int taskIndex, int[] oldStackDepth, out Queue result); + + protected abstract DecompositionStatus OnDecomposeSlot(IContext ctx, Slot task, int taskIndex, int[] oldStackDepth, out Queue result); + + // ========================================================= VALIDITY + + public virtual bool IsValid(IContext ctx) + { + foreach (var condition in Conditions) + { + var result = condition.IsValid(ctx); + if (ctx.LogDecomposition) Log(ctx, $"PrimitiveTask.IsValid:{(result ? "Success" : "Failed")}:{condition.Name} is{(result ? "" : " not")} valid!", result ? ConsoleColor.DarkGreen : ConsoleColor.DarkRed); + if (result == false) + { + return false; + } + } + + return true; + } + + // ========================================================= LOGGING + + protected virtual void Log(IContext ctx, string description, ConsoleColor color = ConsoleColor.White) + { + ctx.Log(Name, description, ctx.CurrentDecompositionDepth, this, color); + } + } +} diff --git a/tools/Fluid-HTN/Tasks/CompoundTasks/DecompositionStatus.cs b/tools/Fluid-HTN/Tasks/CompoundTasks/DecompositionStatus.cs new file mode 100644 index 0000000..71f59b6 --- /dev/null +++ b/tools/Fluid-HTN/Tasks/CompoundTasks/DecompositionStatus.cs @@ -0,0 +1,10 @@ +namespace FluidHTN.Compounds +{ + public enum DecompositionStatus + { + Succeeded, + Partial, + Failed, + Rejected + } +} diff --git a/tools/Fluid-HTN/Tasks/CompoundTasks/ICompoundTask.cs b/tools/Fluid-HTN/Tasks/CompoundTasks/ICompoundTask.cs new file mode 100644 index 0000000..48ec247 --- /dev/null +++ b/tools/Fluid-HTN/Tasks/CompoundTasks/ICompoundTask.cs @@ -0,0 +1,18 @@ + +using System.Collections.Generic; + +namespace FluidHTN.Compounds +{ + public interface ICompoundTask : ITask + { + List Subtasks { get; } + ICompoundTask AddSubtask(ITask subtask); + + /// + /// Decompose the task onto the tasks to process queue, mind it's depth first + /// + /// + /// + DecompositionStatus Decompose(IContext ctx, int startIndex, out Queue result); + } +} diff --git a/tools/Fluid-HTN/Tasks/CompoundTasks/IDecomposeAll.cs b/tools/Fluid-HTN/Tasks/CompoundTasks/IDecomposeAll.cs new file mode 100644 index 0000000..208c48f --- /dev/null +++ b/tools/Fluid-HTN/Tasks/CompoundTasks/IDecomposeAll.cs @@ -0,0 +1,12 @@ +namespace FluidHTN.Compounds +{ + /// + /// The Decompose All interface is a tag to signify that this compound task type intends to + /// decompose all its subtasks. + /// For a task to support Pause Plan tasks, needed for partial planning, it must be + /// a decompose-all compound task type. + /// + public interface IDecomposeAll : ICompoundTask + { + } +} \ No newline at end of file diff --git a/tools/Fluid-HTN/Tasks/CompoundTasks/PausePlanTask.cs b/tools/Fluid-HTN/Tasks/CompoundTasks/PausePlanTask.cs new file mode 100644 index 0000000..ece727b --- /dev/null +++ b/tools/Fluid-HTN/Tasks/CompoundTasks/PausePlanTask.cs @@ -0,0 +1,58 @@ +using System; +using System.Collections.Generic; +using FluidHTN.Compounds; +using FluidHTN.Conditions; + +namespace FluidHTN +{ + public class PausePlanTask : ITask + { + // ========================================================= PROPERTIES + + public string Name { get; set; } + public ICompoundTask Parent { get; set; } + public List Conditions { get; } = null; + public List Effects { get; } = null; + public TaskStatus LastStatus { get; } + + // ========================================================= VALIDITY + + public DecompositionStatus OnIsValidFailed(IContext ctx) + { + return DecompositionStatus.Failed; + } + + // ========================================================= ADDERS + + public ITask AddCondition(ICondition condition) + { + throw new Exception("Pause Plan tasks does not support conditions."); + } + + public ITask AddEffect(IEffect effect) + { + throw new Exception("Pause Plan tasks does not support effects."); + } + + // ========================================================= FUNCTIONALITY + + public void ApplyEffects(IContext ctx) + { + } + + // ========================================================= VALIDITY + + public bool IsValid(IContext ctx) + { + if (ctx.LogDecomposition) Log(ctx, $"PausePlanTask.IsValid:Success!"); + return true; + } + + // ========================================================= LOGGING + + protected virtual void Log(IContext ctx, string description) + { + ctx.Log(Name, description, ctx.CurrentDecompositionDepth, this, ConsoleColor.Green); + } + } +} diff --git a/tools/Fluid-HTN/Tasks/CompoundTasks/Selector.cs b/tools/Fluid-HTN/Tasks/CompoundTasks/Selector.cs new file mode 100644 index 0000000..f3cefbb --- /dev/null +++ b/tools/Fluid-HTN/Tasks/CompoundTasks/Selector.cs @@ -0,0 +1,253 @@ +using System; +using System.Collections.Generic; +using FluidHTN.PrimitiveTasks; + +namespace FluidHTN.Compounds +{ + public class Selector : CompoundTask + { + // ========================================================= FIELDS + + protected readonly Queue Plan = new Queue(); + + // ========================================================= VALIDITY + + public override bool IsValid(IContext ctx) + { + // Check that our preconditions are valid first. + if (base.IsValid(ctx) == false) + { + if (ctx.LogDecomposition) Log(ctx, $"Selector.IsValid:Failed:Preconditions not met!", ConsoleColor.Red); + return false; + } + + // Selector requires there to be at least one sub-task to successfully select from. + if (Subtasks.Count == 0) + { + if (ctx.LogDecomposition) Log(ctx, $"Selector.IsValid:Failed:No sub-tasks!", ConsoleColor.Red); + return false; + } + + if (ctx.LogDecomposition) Log(ctx, $"Selector.IsValid:Success!", ConsoleColor.Green); + return true; + } + + private bool BeatsLastMTR(IContext ctx, int taskIndex, int currentDecompositionIndex) + { + // If the last plan's traversal record for this decomposition layer + // has a smaller index than the current task index we're about to + // decompose, then the new decomposition can't possibly beat the + // running plan, so we cancel finding a new plan. + if (ctx.LastMTR[currentDecompositionIndex] < taskIndex) + { + // But, if any of the earlier records beat the record in LastMTR, we're still good, as we're on a higher priority branch. + // This ensures that [0,0,1] can beat [0,1,0] + for (var i = 0; i < ctx.MethodTraversalRecord.Count; i++) + { + var diff = ctx.MethodTraversalRecord[i] - ctx.LastMTR[i]; + if (diff < 0) + { + return true; + } + if (diff > 0) + { + // We should never really be able to get here, but just in case. + return false; + } + } + + return false; + } + + return true; + } + + // ========================================================= DECOMPOSITION + + /// + /// In a Selector decomposition, just a single sub-task must be valid and successfully decompose for the Selector to be + /// successfully decomposed. + /// + /// + /// + protected override DecompositionStatus OnDecompose(IContext ctx, int startIndex, out Queue result) + { + Plan.Clear(); + + for (var taskIndex = startIndex; taskIndex < Subtasks.Count; taskIndex++) + { + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecompose:Task index: {taskIndex}: {Subtasks[taskIndex]?.Name}"); + // If the last plan is still running, we need to check whether the + // new decomposition can possibly beat it. + if (ctx.LastMTR != null && ctx.LastMTR.Count > 0) + { + if (ctx.MethodTraversalRecord.Count < ctx.LastMTR.Count) + { + var currentDecompositionIndex = ctx.MethodTraversalRecord.Count; + if (BeatsLastMTR(ctx, taskIndex, currentDecompositionIndex) == false) + { + ctx.MethodTraversalRecord.Add(-1); + if (ctx.DebugMTR) ctx.MTRDebug.Add($"REPLAN FAIL {Subtasks[taskIndex].Name}"); + + if (ctx.LogDecomposition) + Log(ctx, + $"Selector.OnDecompose:Rejected:Index {currentDecompositionIndex} is beat by last method traversal record!", ConsoleColor.Red); + result = null; + return DecompositionStatus.Rejected; + } + } + } + + var task = Subtasks[taskIndex]; + + var status = OnDecomposeTask(ctx, task, taskIndex, null, out result); + switch (status) + { + case DecompositionStatus.Rejected: + case DecompositionStatus.Succeeded: + case DecompositionStatus.Partial: + return status; + case DecompositionStatus.Failed: + default: + continue; + } + } + + result = Plan; + return result.Count == 0 ? DecompositionStatus.Failed : DecompositionStatus.Succeeded; + } + + protected override DecompositionStatus OnDecomposeTask(IContext ctx, ITask task, int taskIndex, + int[] oldStackDepth, out Queue result) + { + if (task.IsValid(ctx) == false) + { + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecomposeTask:Failed:Task {task.Name}.IsValid returned false!", ConsoleColor.Red); + result = Plan; + return task.OnIsValidFailed(ctx); + } + + if (task is ICompoundTask compoundTask) + { + return OnDecomposeCompoundTask(ctx, compoundTask, taskIndex, null, out result); + } + + if (task is IPrimitiveTask primitiveTask) + { + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecomposeTask:Pushed {primitiveTask.Name} to plan!", ConsoleColor.Blue); + primitiveTask.ApplyEffects(ctx); + Plan.Enqueue(task); + } + + if (task is Slot slot) + { + return OnDecomposeSlot(ctx, slot, taskIndex, null, out result); + } + + result = Plan; + var status = result.Count == 0 ? DecompositionStatus.Failed : DecompositionStatus.Succeeded; + + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecomposeTask:{status}!", status == DecompositionStatus.Succeeded ? ConsoleColor.Green : ConsoleColor.Red); + return status; + } + + protected override DecompositionStatus OnDecomposeCompoundTask(IContext ctx, ICompoundTask task, int taskIndex, + int[] oldStackDepth, out Queue result) + { + // We need to record the task index before we decompose the task, + // so that the traversal record is set up in the right order. + ctx.MethodTraversalRecord.Add(taskIndex); + if (ctx.DebugMTR) ctx.MTRDebug.Add(task.Name); + + var status = task.Decompose(ctx, 0, out var subPlan); + + // If status is rejected, that means the entire planning procedure should cancel. + if (status == DecompositionStatus.Rejected) + { + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecomposeCompoundTask:{status}: Decomposing {task.Name} was rejected.", ConsoleColor.Red); + result = null; + return DecompositionStatus.Rejected; + } + + // If the decomposition failed + if (status == DecompositionStatus.Failed) + { + // Remove the taskIndex if it failed to decompose. + ctx.MethodTraversalRecord.RemoveAt(ctx.MethodTraversalRecord.Count - 1); + if (ctx.DebugMTR) ctx.MTRDebug.RemoveAt(ctx.MTRDebug.Count - 1); + + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecomposeCompoundTask:{status}: Decomposing {task.Name} failed.", ConsoleColor.Red); + result = Plan; + return DecompositionStatus.Failed; + } + + while (subPlan.Count > 0) + { + var p = subPlan.Dequeue(); + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecomposeCompoundTask:Decomposing {task.Name}:Pushed {p.Name} to plan!", ConsoleColor.Blue); + Plan.Enqueue(p); + } + + if (ctx.HasPausedPartialPlan) + { + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecomposeCompoundTask:Return partial plan at index {taskIndex}!", ConsoleColor.DarkBlue); + result = Plan; + return DecompositionStatus.Partial; + } + + result = Plan; + var s = result.Count == 0 ? DecompositionStatus.Failed : DecompositionStatus.Succeeded; + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecomposeCompoundTask:{s}!", s == DecompositionStatus.Succeeded ? ConsoleColor.Green : ConsoleColor.Red); + return s; + } + + protected override DecompositionStatus OnDecomposeSlot(IContext ctx, Slot task, int taskIndex, int[] oldStackDepth, out Queue result) + { + // We need to record the task index before we decompose the task, + // so that the traversal record is set up in the right order. + ctx.MethodTraversalRecord.Add(taskIndex); + if (ctx.DebugMTR) ctx.MTRDebug.Add(task.Name); + + var status = task.Decompose(ctx, 0, out var subPlan); + + // If status is rejected, that means the entire planning procedure should cancel. + if (status == DecompositionStatus.Rejected) + { + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecomposeSlot:{status}: Decomposing {task.Name} was rejected.", ConsoleColor.Red); + result = null; + return DecompositionStatus.Rejected; + } + + // If the decomposition failed + if (status == DecompositionStatus.Failed) + { + // Remove the taskIndex if it failed to decompose. + ctx.MethodTraversalRecord.RemoveAt(ctx.MethodTraversalRecord.Count - 1); + if (ctx.DebugMTR) ctx.MTRDebug.RemoveAt(ctx.MTRDebug.Count - 1); + + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecomposeSlot:{status}: Decomposing {task.Name} failed.", ConsoleColor.Red); + result = Plan; + return DecompositionStatus.Failed; + } + + while (subPlan.Count > 0) + { + var p = subPlan.Dequeue(); + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecomposeSlot:Decomposing {task.Name}:Pushed {p.Name} to plan!", ConsoleColor.Blue); + Plan.Enqueue(p); + } + + if (ctx.HasPausedPartialPlan) + { + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecomposeSlot:Return partial plan!", ConsoleColor.DarkBlue); + result = Plan; + return DecompositionStatus.Partial; + } + + result = Plan; + var s = result.Count == 0 ? DecompositionStatus.Failed : DecompositionStatus.Succeeded; + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecomposeSlot:{s}!", s == DecompositionStatus.Succeeded ? ConsoleColor.Green : ConsoleColor.Red); + return s; + } + } +} diff --git a/tools/Fluid-HTN/Tasks/CompoundTasks/Sequence.cs b/tools/Fluid-HTN/Tasks/CompoundTasks/Sequence.cs new file mode 100644 index 0000000..971bcb4 --- /dev/null +++ b/tools/Fluid-HTN/Tasks/CompoundTasks/Sequence.cs @@ -0,0 +1,231 @@ +using System; +using System.Collections.Generic; +using FluidHTN.PrimitiveTasks; + +namespace FluidHTN.Compounds +{ + public class Sequence : CompoundTask, IDecomposeAll + { + // ========================================================= FIELDS + + protected readonly Queue Plan = new Queue(); + + // ========================================================= VALIDITY + + public override bool IsValid(IContext ctx) + { + // Check that our preconditions are valid first. + if (base.IsValid(ctx) == false) + { + if (ctx.LogDecomposition) Log(ctx, $"Sequence.IsValid:Failed:Preconditions not met!", ConsoleColor.Red); + return false; + } + + // Selector requires there to be subtasks to successfully select from. + if (Subtasks.Count == 0) + { + if (ctx.LogDecomposition) Log(ctx, $"Sequence.IsValid:Failed:No sub-tasks!", ConsoleColor.Red); + return false; + } + + if (ctx.LogDecomposition) Log(ctx, $"Sequence.IsValid:Success!", ConsoleColor.Green); + return true; + } + + // ========================================================= DECOMPOSITION + + /// + /// In a Sequence decomposition, all sub-tasks must be valid and successfully decomposed in order for the Sequence to + /// be successfully decomposed. + /// + /// + /// + protected override DecompositionStatus OnDecompose(IContext ctx, int startIndex, out Queue result) + { + Plan.Clear(); + + var oldStackDepth = ctx.GetWorldStateChangeDepth(ctx.Factory); + + for (var taskIndex = startIndex; taskIndex < Subtasks.Count; taskIndex++) + { + var task = Subtasks[taskIndex]; + if (ctx.LogDecomposition) Log(ctx, $"Selector.OnDecompose:Task index: {taskIndex}: {task?.Name}"); + + var status = OnDecomposeTask(ctx, task, taskIndex, oldStackDepth, out result); + switch (status) + { + case DecompositionStatus.Rejected: + case DecompositionStatus.Failed: + case DecompositionStatus.Partial: + { + ctx.Factory.FreeArray(ref oldStackDepth); + return status; + } + } + } + + ctx.Factory.FreeArray(ref oldStackDepth); + + result = Plan; + return result.Count == 0 ? DecompositionStatus.Failed : DecompositionStatus.Succeeded; + } + + protected override DecompositionStatus OnDecomposeTask(IContext ctx, ITask task, int taskIndex, + int[] oldStackDepth, out Queue result) + { + if (task.IsValid(ctx) == false) + { + if (ctx.LogDecomposition) Log(ctx, $"Sequence.OnDecomposeTask:Failed:Task {task.Name}.IsValid returned false!", ConsoleColor.Red); + Plan.Clear(); + ctx.TrimToStackDepth(oldStackDepth); + result = Plan; + return task.OnIsValidFailed(ctx); + } + + if (task is ICompoundTask compoundTask) + { + return OnDecomposeCompoundTask(ctx, compoundTask, taskIndex, oldStackDepth, out result); + } + else if (task is IPrimitiveTask primitiveTask) + { + if (ctx.LogDecomposition) Log(ctx, $"Sequence.OnDecomposeTask:Pushed {primitiveTask.Name} to plan!", ConsoleColor.Blue); + primitiveTask.ApplyEffects(ctx); + Plan.Enqueue(task); + } + else if (task is PausePlanTask) + { + if (ctx.LogDecomposition) Log(ctx, $"Sequence.OnDecomposeTask:Return partial plan at index {taskIndex}!", ConsoleColor.DarkBlue); + ctx.HasPausedPartialPlan = true; + ctx.PartialPlanQueue.Enqueue(new PartialPlanEntry() + { + Task = this, + TaskIndex = taskIndex + 1, + }); + + result = Plan; + return DecompositionStatus.Partial; + } + else if (task is Slot slot) + { + return OnDecomposeSlot(ctx, slot, taskIndex, oldStackDepth, out result); + } + + result = Plan; + var s = result.Count == 0 ? DecompositionStatus.Failed : DecompositionStatus.Succeeded; + if (ctx.LogDecomposition) Log(ctx, $"Sequence.OnDecomposeTask:{s}!", s == DecompositionStatus.Succeeded ? ConsoleColor.Green : ConsoleColor.Red); + return s; + } + + protected override DecompositionStatus OnDecomposeCompoundTask(IContext ctx, ICompoundTask task, + int taskIndex, int[] oldStackDepth, out Queue result) + { + var status = task.Decompose(ctx, 0, out var subPlan); + + // If result is null, that means the entire planning procedure should cancel. + if (status == DecompositionStatus.Rejected) + { + if (ctx.LogDecomposition) Log(ctx, $"Sequence.OnDecomposeCompoundTask:{status}: Decomposing {task.Name} was rejected.", ConsoleColor.Red); + + Plan.Clear(); + ctx.TrimToStackDepth(oldStackDepth); + + result = null; + return DecompositionStatus.Rejected; + } + + // If the decomposition failed + if (status == DecompositionStatus.Failed) + { + if (ctx.LogDecomposition) Log(ctx, $"Sequence.OnDecomposeCompoundTask:{status}: Decomposing {task.Name} failed.", ConsoleColor.Red); + + Plan.Clear(); + ctx.TrimToStackDepth(oldStackDepth); + result = Plan; + return DecompositionStatus.Failed; + } + + while (subPlan.Count > 0) + { + var p = subPlan.Dequeue(); + if (ctx.LogDecomposition) Log(ctx, $"Sequence.OnDecomposeCompoundTask:Decomposing {task.Name}:Pushed {p.Name} to plan!", ConsoleColor.Blue); + Plan.Enqueue(p); + } + + if (ctx.HasPausedPartialPlan) + { + if (ctx.LogDecomposition) Log(ctx, $"Sequence.OnDecomposeCompoundTask:Return partial plan at index {taskIndex}!", ConsoleColor.DarkBlue); + if (taskIndex < Subtasks.Count - 1) + { + ctx.PartialPlanQueue.Enqueue(new PartialPlanEntry() + { + Task = this, + TaskIndex = taskIndex + 1, + }); + } + + result = Plan; + return DecompositionStatus.Partial; + } + + result = Plan; + if (ctx.LogDecomposition) Log(ctx, $"Sequence.OnDecomposeCompoundTask:Succeeded!", ConsoleColor.Green); + return DecompositionStatus.Succeeded; + } + + protected override DecompositionStatus OnDecomposeSlot(IContext ctx, Slot task, + int taskIndex, int[] oldStackDepth, out Queue result) + { + var status = task.Decompose(ctx, 0, out var subPlan); + + // If result is null, that means the entire planning procedure should cancel. + if (status == DecompositionStatus.Rejected) + { + if (ctx.LogDecomposition) Log(ctx, $"Sequence.OnDecomposeSlot:{status}: Decomposing {task.Name} was rejected.", ConsoleColor.Red); + + Plan.Clear(); + ctx.TrimToStackDepth(oldStackDepth); + + result = null; + return DecompositionStatus.Rejected; + } + + // If the decomposition failed + if (status == DecompositionStatus.Failed) + { + if (ctx.LogDecomposition) Log(ctx, $"Sequence.OnDecomposeSlot:{status}: Decomposing {task.Name} failed.", ConsoleColor.Red); + + Plan.Clear(); + ctx.TrimToStackDepth(oldStackDepth); + result = Plan; + return DecompositionStatus.Failed; + } + + while (subPlan.Count > 0) + { + var p = subPlan.Dequeue(); + if (ctx.LogDecomposition) Log(ctx, $"Sequence.OnDecomposeSlot:Decomposing {task.Name}:Pushed {p.Name} to plan!", ConsoleColor.Blue); + Plan.Enqueue(p); + } + + if (ctx.HasPausedPartialPlan) + { + if (ctx.LogDecomposition) Log(ctx, $"Sequence.OnDecomposeSlot:Return partial plan at index {taskIndex}!", ConsoleColor.DarkBlue); + if (taskIndex < Subtasks.Count - 1) + { + ctx.PartialPlanQueue.Enqueue(new PartialPlanEntry() + { + Task = this, + TaskIndex = taskIndex + 1, + }); + } + + result = Plan; + return DecompositionStatus.Partial; + } + + result = Plan; + if (ctx.LogDecomposition) Log(ctx, $"Sequence.OnDecomposeSlot:Succeeded!", ConsoleColor.Green); + return DecompositionStatus.Succeeded; + } + } +} diff --git a/tools/Fluid-HTN/Tasks/CompoundTasks/TaskRoot.cs b/tools/Fluid-HTN/Tasks/CompoundTasks/TaskRoot.cs new file mode 100644 index 0000000..910e396 --- /dev/null +++ b/tools/Fluid-HTN/Tasks/CompoundTasks/TaskRoot.cs @@ -0,0 +1,8 @@ +using FluidHTN.Compounds; + +namespace FluidHTN +{ + public class TaskRoot : Selector + { + } +} \ No newline at end of file diff --git a/tools/Fluid-HTN/Tasks/ITask.cs b/tools/Fluid-HTN/Tasks/ITask.cs new file mode 100644 index 0000000..7a85425 --- /dev/null +++ b/tools/Fluid-HTN/Tasks/ITask.cs @@ -0,0 +1,45 @@ +using System.Collections.Generic; +using FluidHTN.Compounds; +using FluidHTN.Conditions; + +namespace FluidHTN +{ + public interface ITask + { + /// + /// Used for debugging and identification purposes + /// + string Name { get; set; } + + /// + /// The parent of this task in the hierarchy + /// + ICompoundTask Parent { get; set; } + + /// + /// The conditions that must be satisfied for this task to pass as valid. + /// + List Conditions { get; } + + /// + /// Last status returned by Update + /// + TaskStatus LastStatus { get; } + + /// + /// Add a new condition to the task. + /// + /// + /// + ITask AddCondition(ICondition condition); + + /// + /// Check the task's preconditions, returns true if all preconditions are valid. + /// + /// + /// + bool IsValid(IContext ctx); + + DecompositionStatus OnIsValidFailed(IContext ctx); + } +} diff --git a/tools/Fluid-HTN/Tasks/OtherTasks/Slot.cs b/tools/Fluid-HTN/Tasks/OtherTasks/Slot.cs new file mode 100644 index 0000000..014ed48 --- /dev/null +++ b/tools/Fluid-HTN/Tasks/OtherTasks/Slot.cs @@ -0,0 +1,79 @@ +using System; +using System.Collections.Generic; +using FluidHTN.Conditions; + +namespace FluidHTN.Compounds +{ + public class Slot : ITask + { + // ========================================================= PROPERTIES + + public int SlotId { get; set; } + public string Name { get; set; } + public ICompoundTask Parent { get; set; } + public List Conditions { get; } = null; + public TaskStatus LastStatus { get; private set; } + public ICompoundTask Subtask { get; private set; } = null; + + // ========================================================= VALIDITY + + public DecompositionStatus OnIsValidFailed(IContext ctx) + { + return DecompositionStatus.Failed; + } + + // ========================================================= ADDERS + + public ITask AddCondition(ICondition condition) + { + throw new Exception("Slot tasks does not support conditions."); + } + + // ========================================================= SET / REMOVE + + public bool Set(ICompoundTask subtask) + { + if(Subtask != null) + { + return false; + } + + Subtask = subtask; + return true; + } + + public void Clear() + { + Subtask = null; + } + + // ========================================================= DECOMPOSITION + + public DecompositionStatus Decompose(IContext ctx, int startIndex, out Queue result) + { + if(Subtask != null) + { + return Subtask.Decompose(ctx, startIndex, out result); + } + + result = null; + return DecompositionStatus.Failed; + } + + // ========================================================= VALIDITY + + public virtual bool IsValid(IContext ctx) + { + var result = Subtask != null; + if (ctx.LogDecomposition) Log(ctx, $"Slot.IsValid:{(result ? "Success" : "Failed")}!", result ? ConsoleColor.Green : ConsoleColor.Red); + return result; + } + + // ========================================================= LOGGING + + protected virtual void Log(IContext ctx, string description, ConsoleColor color = ConsoleColor.White) + { + ctx.Log(Name, description, ctx.CurrentDecompositionDepth, this, color); + } + } +} diff --git a/tools/Fluid-HTN/Tasks/PrimitiveTasks/IPrimitiveTask.cs b/tools/Fluid-HTN/Tasks/PrimitiveTasks/IPrimitiveTask.cs new file mode 100644 index 0000000..8372b2d --- /dev/null +++ b/tools/Fluid-HTN/Tasks/PrimitiveTasks/IPrimitiveTask.cs @@ -0,0 +1,31 @@ +using System.Collections.Generic; +using FluidHTN.Conditions; +using FluidHTN.Operators; + +namespace FluidHTN.PrimitiveTasks +{ + public interface IPrimitiveTask : ITask + { + /// + /// Executing conditions are validated before every call to Operator.Update(...) + /// + List ExecutingConditions { get; } + + /// + /// Add a new executing condition to the primitive task. This will be checked before + /// every call to Operator.Update(...) + /// + /// + /// + ITask AddExecutingCondition(ICondition condition); + + IOperator Operator { get; } + void SetOperator(IOperator action); + + List Effects { get; } + ITask AddEffect(IEffect effect); + void ApplyEffects(IContext ctx); + + void Stop(IContext ctx); + } +} \ No newline at end of file diff --git a/tools/Fluid-HTN/Tasks/PrimitiveTasks/PrimitiveTask.cs b/tools/Fluid-HTN/Tasks/PrimitiveTasks/PrimitiveTask.cs new file mode 100644 index 0000000..e0777c9 --- /dev/null +++ b/tools/Fluid-HTN/Tasks/PrimitiveTasks/PrimitiveTask.cs @@ -0,0 +1,108 @@ +using System; +using System.Collections.Generic; +using FluidHTN.Compounds; +using FluidHTN.Conditions; +using FluidHTN.Operators; + +namespace FluidHTN.PrimitiveTasks +{ + public class PrimitiveTask : IPrimitiveTask + { + // ========================================================= PROPERTIES + + public string Name { get; set; } + public ICompoundTask Parent { get; set; } + public List Conditions { get; } = new List(); + public List ExecutingConditions { get; } = new List(); + public TaskStatus LastStatus { get; } + public IOperator Operator { get; private set; } + public List Effects { get; } = new List(); + + // ========================================================= VALIDITY + + public DecompositionStatus OnIsValidFailed(IContext ctx) + { + return DecompositionStatus.Failed; + } + + // ========================================================= ADDERS + + public ITask AddCondition(ICondition condition) + { + Conditions.Add(condition); + return this; + } + + public ITask AddExecutingCondition(ICondition condition) + { + ExecutingConditions.Add(condition); + return this; + } + + public ITask AddEffect(IEffect effect) + { + Effects.Add(effect); + return this; + } + + // ========================================================= SETTERS + + public void SetOperator(IOperator action) + { + if (Operator != null) throw new Exception("A Primitive Task can only contain a single Operator!"); + + Operator = action; + } + + // ========================================================= FUNCTIONALITY + + public void ApplyEffects(IContext ctx) + { + if (ctx.ContextState == ContextState.Planning) + { + if (ctx.LogDecomposition) Log(ctx, $"PrimitiveTask.ApplyEffects", ConsoleColor.Yellow); + } + + if (ctx.LogDecomposition) ctx.CurrentDecompositionDepth++; + foreach (var effect in Effects) + { + effect.Apply(ctx); + } + if (ctx.LogDecomposition) ctx.CurrentDecompositionDepth--; + } + + public void Stop(IContext ctx) + { + Operator?.Stop(ctx); + } + + // ========================================================= VALIDITY + + public bool IsValid(IContext ctx) + { + if (ctx.LogDecomposition) Log(ctx, $"PrimitiveTask.IsValid check"); + foreach (var condition in Conditions) + { + if (ctx.LogDecomposition) ctx.CurrentDecompositionDepth++; + var result = condition.IsValid(ctx); + if (ctx.LogDecomposition) ctx.CurrentDecompositionDepth--; + if (ctx.LogDecomposition) Log(ctx, $"PrimitiveTask.IsValid:{(result ? "Success" : "Failed")}:{condition.Name} is{(result ? "" : " not")} valid!", result ? ConsoleColor.DarkGreen : ConsoleColor.DarkRed); + if (result == false) + { + if (ctx.LogDecomposition) Log(ctx, $"PrimitiveTask.IsValid:Failed:Preconditions not met!", ConsoleColor.Red); + return false; + } + } + + if (ctx.LogDecomposition) Log(ctx, $"PrimitiveTask.IsValid:Success!", ConsoleColor.Green); + return true; + } + + // ========================================================= LOGGING + + protected virtual void Log(IContext ctx, string description, ConsoleColor color = ConsoleColor.White) + { + ctx.Log(Name, description, ctx.CurrentDecompositionDepth+1, this, color); + } + } +} diff --git a/tools/Fluid-HTN/Tasks/TaskStatus.cs b/tools/Fluid-HTN/Tasks/TaskStatus.cs new file mode 100644 index 0000000..ec68180 --- /dev/null +++ b/tools/Fluid-HTN/Tasks/TaskStatus.cs @@ -0,0 +1,9 @@ +namespace FluidHTN +{ + public enum TaskStatus + { + Continue, + Success, + Failure + } +} \ No newline at end of file diff --git a/tools/Fluid-HTN/package.json b/tools/Fluid-HTN/package.json new file mode 100644 index 0000000..72e83ab --- /dev/null +++ b/tools/Fluid-HTN/package.json @@ -0,0 +1,9 @@ +{ + "name": "fluid.htn", + "displayName": "Fluid HTN", + "version": "0.1.0", + "unity": "2019.1", + "author": "Pål Trefall", + "description": "HTN Planner Framework", + "dependencies": {} +}