diff --git a/src/planners/backward.jl b/src/planners/backward.jl index 556917d..030965f 100644 --- a/src/planners/backward.jl +++ b/src/planners/backward.jl @@ -148,8 +148,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 diff --git a/src/planners/bfs.jl b/src/planners/bfs.jl index b19b6e7..0b49d31 100644 --- a/src/planners/bfs.jl +++ b/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 diff --git a/src/planners/forward.jl b/src/planners/forward.jl index f901ced..7e8f27d 100644 --- a/src/planners/forward.jl +++ b/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 @@ -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)