Fix constraint checking in Forward and BreadthFirstSearch planners

src/planners/backward.jl

@@ -38,7 +38,8 @@ backward search.
 Returns a [`PathSearchSolution`](@ref) or [`NullSolution`](@ref), similar to
 [`ForwardPlanner`](@ref).
 
-This planner does not currently support domains with non-Boolean fluents.
+This planner does not currently support domains with non-Boolean fluents or 
+problems involving constraint specifications.
 
 [1] B. Bonet and H. Geffner, "Planning as Heuristic Search," Artificial
 Intelligence, vol. 129, no. 1, pp. 5–33, Jun. 2001,
@@ -148,8 +149,12 @@ function solve(planner::BackwardPlanner,
     search_order = UInt[]
     sol = PathSearchSolution(:in_progress, Term[], Vector{typeof(state)}(),
                              0, search_tree, queue, search_order)
-    # Run the search
-    sol = search!(sol, planner, planner.heuristic, domain, spec)
+    # Check if initial state satisfies trajectory constraints
+    if is_violated(spec, domain, state)
+        sol.status = :failure
+    else # Run the search
+        sol = search!(sol, planner, planner.heuristic, domain, spec)
+    end
     # Return solution
     if save_search
         return sol
@@ -221,7 +226,7 @@ function expand!(
         # Regress (reverse-execute) the action
         next_state = regress(domain, state, act; check=false)
         # Add constraints to regression state
-        add_constraints!(spec, domain, state)
+        add_constraints!(spec, domain, next_state)
         next_id = hash(next_state)
         # Compute path cost
         act_cost = get_cost(spec, domain, state, act, next_state)

src/planners/bfs.jl

@@ -57,8 +57,12 @@ function solve(planner::BreadthFirstPlanner,
     search_order = UInt[]
     sol = PathSearchSolution(:in_progress, Term[], Vector{typeof(state)}(),
                              0, search_tree, queue, search_order)
-    # Run the search
-    sol = search!(sol, planner, domain, spec)
+    # Check if initial state satisfies trajectory constraints
+    if is_violated(spec, domain, state)
+        sol.status = :failure
+    else # Run the search
+        sol = search!(sol, planner, domain, spec)
+    end
     # Return solution
     if save_search
         return sol
@@ -126,7 +130,7 @@ function expand!(
         # Skip if state has already been encountered
         if haskey(search_tree, next_id) continue end
         # Check if next state satisfies trajectory constraints
-        if is_violated(spec, domain, state) continue end
+        if is_violated(spec, domain, next_state) continue end
         # Update backpointer and add next state to queue
         path_cost = node.path_cost + 1
         search_tree[next_id] = PathNode(next_id, next_state, path_cost,

src/planners/forward.jl

@@ -206,8 +206,12 @@ function solve(planner::ForwardPlanner,
     precompute!(heuristic, domain, state, spec)
     # Initialize solution
     sol = init_sol(planner, heuristic, domain, state, spec)
-    # Run the search
-    sol = search!(sol, planner, heuristic, domain, spec)
+    # Check if initial state satisfies trajectory constraints
+    if is_violated(spec, domain, state)
+        sol.status = :failure
+    else # Run the search
+        sol = search!(sol, planner, heuristic, domain, spec)
+    end
     # Return solution
     if save_search
         return sol
@@ -323,7 +327,7 @@ function expand!(
         next_state = transition(domain, state, act; check=false)
         next_id = hash(next_state)
         # Check if next state satisfies trajectory constraints
-        if is_violated(spec, domain, state) continue end
+        if is_violated(spec, domain, next_state) continue end
         # Compute path cost
         act_cost = get_cost(spec, domain, state, act, next_state)
         path_cost = node.path_cost + act_cost
@@ -378,6 +382,11 @@ function refine!(
     # Decide between restarting, rerooting, or continuing the search
     if refine_method == :restart
         (sol.status == :failure && is_reached(state, sol)) && return sol
+        # Check if initial state satisfies trajectory constraints
+        if is_violated(spec, domain, state) 
+            sol.status = :failure
+            return sol
+        end
         reinit_sol!(sol, planner, heuristic, domain, state, spec)
     elseif refine_method == :reroot
         reroot!(sol, planner, heuristic, domain, state, spec)