def main():
# Code for use in paging and debugging
from check_result import check_result, pause, set_trace
# If we've changed to some other domain, this will change us back.
pyhop2.set_current_domain(domain_name)
pyhop2.print_domain()
state1 = state0.copy()
state1.display(heading='\nInitial state is')
pause()
print("-- If verbose=3, the planner will print even more information.\n")
start1 = timer()
result = pyhop2.find_plan_GBFS(state1, [('make_string', 'END')],
h_BFS,
verbose=3)
end1 = timer()
print(end1 - start1)
start2 = timer()
result = pyhop2.find_plan_a_star(state1, [('make_string', 'END')],
h_BFS,
verbose=3)
end2 = timer()
print(end2 - start2)
def main():
# Code for use in paging and debugging
from check_result import check_result, pause, set_trace
# If we've changed to some other domain, this will change us back.
pyhop2.set_current_domain(domain_name)
pyhop2.print_domain()
state0a.display(heading='\nInitial state')
pause()
print("Use run_lazy_lookahead to get Alice to the park.\n")
pyhop2.run_lazy_lookahead(state0a, [('travel', 'alice', 'park')],
verbose=1)
print('')
pause()
print("""
Next is a demonstration of what can happen if your HTN methods are too brittle.
We'll try to use run_lazy_lookahead to get Alice to the park, but the taxi will
break down while Alice is in it. run_lazy_lookahead will call find_plan again,
but find_plan will fail because the HTN methods don't handle this case.
""")
state0b.display(heading='The initial state is')
print('Next, the call to run_lazy_lookahead ...')
pause()
pyhop2.run_lazy_lookahead(state0b, [('travel', 'alice', 'park')],
verbose=1)
pause()
print("No more examples")
def main():
# Code for use in paging and debugging
from check_result import check_result, pause, set_trace
# If we've changed to some other domain, this will change us back.
pyhop2.set_current_domain(domain_name)
# pyhop2.print_domain()
# state0.display(heading='\nInitial state is')
# goal.display(heading='\nGoal is')
avgCost, avgTime = test(h_moves, c_dist, size=15, iter=500)
print(avgCost, avgTime)
def main():
# Code for use in paging and debugging
from check_result import check_result, pause, set_trace
# If we've changed to some other domain, this will change us back.
pyhop2.set_current_domain(domain_name)
pyhop2.print_domain()
state1 = state0.copy()
state1.display(heading='\nInitial state is')
# two possible expected answers for check_result
expect0 = [('putv', 0), ('getv', 0), ('getv', 0)]
expect1 = [('putv', 1), ('getv', 1), ('getv', 1)]
print(
"-- Four examples with verbose=3 to get a detailed account of the backtracking."
)
# Below, the comma after each task name is so Python will parse the task
# as a tuple rather than an atom
result = pyhop2.find_plan(state0, [('put_it', ), ('need0', )], verbose=3)
check_result(result, expect0)
print(
"""Above, seek_plan backtracks once to use a different method for 'put_it'.
""")
pause()
result = pyhop2.find_plan(state0, [('put_it', ), ('need01', )], verbose=3)
check_result(result, expect0)
print(
"""The backtracking in the above example is the same as in the first one.
""")
pause()
result = pyhop2.find_plan(state0, [('put_it', ), ('need10', )], verbose=3)
check_result(result, expect0)
print(
"""Above, seek_plan backtracks to use a different method for 'put_it',
and later it backtracks to use a different method for 'need10'.
""")
pause()
result = pyhop2.find_plan(state0, [('put_it', ), ('need1', )], verbose=3)
check_result(result, expect1)
print(
"""First, seek_plan backtracks to use a different method for 'put_it'. But the
solution it finds for 'put_it' doesn't satisfy the preconditions of the method
for 'need1', making it backtrack to use a third method for 'put_it'.
""")
pyhop2.declare_task_methods('groceryshop', do_nothing, shop)
pyhop2.declare_task_methods('pay', paid, pay_money)
# Running the examples
print('-----------------------------------------------------------------------')
print(f"Created the domain '{domain_name}'.")
############################
##### GROCERY PLAN ENDS ####
############################
if __name__ == '__main__':
pyhop2.set_current_domain(domain_name)
pyhop2.print_domain()
node = rospy.init_node('a_star_planner')
rospy.wait_for_service('follow_path')
step_size = 0.4
try:
rospy.set_param("turtlebot/step_size", step_size)
follow_path = rospy.ServiceProxy('follow_path', Path)
robot_location = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
# resp_location = robot_location('mobile_base', 'world')
# coord = (resp_location.pose.position.x, resp_location.pose.position.y)
#####################################################
def main():
# Code for use in paging and debugging
from check_result import check_result, pause, set_trace
# If we've changed to some other domain, this will change us back.
pyhop2.set_current_domain(domain_name)
pyhop2.print_domain()
state1 = state0.copy()
# state1.display(heading='\nThe initial state is')
print("""
Next, several planning problems using the above domain and initial state.
""")
pause()
print("""
Below, we give find_plan the goal of having alice be at the park.
We do it several times with different values for 'verbose'.
""")
expected = [('call_taxi', 'alice', 'home_a'),
('ride_taxi', 'alice', 'park'),
('pay_driver', 'alice', 'park')]
print("If verbose=0, the planner returns the solution but prints nothing:")
result = pyhop2.find_plan(state1, [('loc', 'alice', 'park')], verbose=0)
check_result(result, expected)
print(
"""If verbose=1, then in addition to returning the solution, the planner prints
both the problem and the solution"
""")
result = pyhop2.find_plan(state1, [('loc', 'alice', 'park')], verbose=1)
check_result(result, expected)
print(
"""If verbose=2, the planner also prints a note at each recursive call. Below,
_verify_g is a task used by the planner to check whether a method has
achieved its goal.
""")
result = pyhop2.find_plan(state1, [('loc', 'alice', 'park')], verbose=2)
check_result(result, expected)
pause()
print("""
If verbose=3, the planner prints even more information.
""")
result = pyhop2.find_plan(state1, [('loc', 'alice', 'park')], verbose=3)
check_result(result, expected)
pause()
print("""
Next, we give find_plan a sequence of two goals: first for Alice to be at the
park, then for Bob to be at the park. Since this is a sequence, it doesn't
matter whether they're both at the park at the same time.
""")
plan = pyhop2.find_plan(state1, [('loc', 'alice', 'park'),
('loc', 'bob', 'park')],
verbose=2)
check_result(plan, [('call_taxi', 'alice', 'home_a'),
('ride_taxi', 'alice', 'park'),
('pay_driver', 'alice', 'park'),
('walk', 'bob', 'home_b', 'park')])
pause()
state1.display(heading='\nInitial state')
print("""
A multigoal g looks similar to a state, but usually it includes just a few of
the state variables rather than all of them. It specifies *desired* values
for those state variables, rather than current values. The goal is to produce
a state that satisfies all of the desired values.
Below, goal3 is the goal of having Alice and Bob at the park at the same time.
""")
goal3.display()
print("""
Next, we'll call find_plan on goal3, with verbose=2. In the printout,
_verify_mg is a task used by the planner to check whether a multigoal
method has achieved all of the values specified in the multigoal.
""")
pause()
plan = pyhop2.find_plan(state1, [goal3], verbose=2)
check_result(plan, [('call_taxi', 'alice', 'home_a'),
('ride_taxi', 'alice', 'park'),
('pay_driver', 'alice', 'park'),
('walk', 'bob', 'home_b', 'park')])
pause()
print('\nCall run_lazy_lookahead with verbose=1:\n')
new_state = pyhop2.run_lazy_lookahead(state1, [('loc', 'alice', 'park')],
verbose=1)
print('')
pause()
print(
'\nAlice is now at the park, so the planner will return an empty plan:\n'
)
plan = pyhop2.find_plan(new_state, [('loc', 'alice', 'park')], verbose=1)
check_result(plan, [])
print("No more examples")
def main():
# If we've changed to some other domain, this will change us back.
print(f"Changing current domain to {__name__}, if it isn't that already.")
pyhop2.set_current_domain(__name__)
pyhop2.print_domain()
print("\nLet's call find_plan on some simple things that should fail.\n")
state1 = pyhop2.State('state1')
state1.pos = {'a': 'b', 'b': 'table', 'c': 'table'}
state1.clear = {'c': True, 'b': False, 'a': True}
state1.holding = {'hand': False}
state1.display('Initial state is')
plan = pyhop2.find_plan(state1, [('pickup', 'a')], verbose=1)
check_result(plan, False)
plan = pyhop2.find_plan(state1, [('pickup', 'b')], verbose=1)
check_result(plan, False)
plan = pyhop2.find_plan(state1, [('pos', 'b', 'hand')], verbose=1)
check_result(plan, False)
pause()
print("""
Next, some simple things that should succeed. As a reminder, in state1,
block a is on block b, block b is on the table, and block c is on the table.
""")
plan = pyhop2.find_plan(state1, [('pickup', 'c')], verbose=1)
check_result(plan, [('pickup', 'c')])
plan = pyhop2.find_plan(state1, [('unstack', 'a', 'b')], verbose=1)
check_result(plan, [('unstack', 'a', 'b')])
plan = pyhop2.find_plan(state1, [('pos', 'a', 'hand')], verbose=1)
check_result(plan, [('unstack', 'a', 'b')])
plan = pyhop2.find_plan(state1, [('pos', 'c', 'hand')], verbose=1)
check_result(plan, [('pickup', 'c')])
plan = pyhop2.find_plan(state1, [('pos', 'a', 'table')], verbose=1)
check_result(plan, [('unstack', 'a', 'b'), ('putdown', 'a')])
pause()
print("""
A Multigoal is a data structure that specifies desired values for some of the
state variables. In blocks_tasks.py there are examples of tasks whose arguments
are multigoals, but here we give the multigoals directly to find_plan.
Below, goal1a says we want the blocks in the configuration "c on b on a", and
goal1a says we want "c on b on a on the table". goal1a and goal1b have the same
set of solutions, because the "a on the table" will be achieved anyway.
""")
state1.display("Initial state is")
goal1a = pyhop2.Multigoal('goal1a')
goal1a.pos = {'c': 'b', 'b': 'a', 'a': 'table'}
goal1a.display()
goal1b = pyhop2.Multigoal('goal1b')
goal1b.pos = {'c': 'b', 'b': 'a'}
goal1b.display()
expected = [('unstack', 'a', 'b'), ('putdown', 'a'), ('pickup', 'b'),
('stack', 'b', 'a'), ('pickup', 'c'), ('stack', 'c', 'b')]
plan1 = pyhop2.find_plan(state1, [goal1a], verbose=1)
check_result(plan1, expected)
plan2 = pyhop2.find_plan(state1, [goal1b], verbose=1)
check_result(plan2, expected)
pause()
print("""
Run find_plan on two more planning problems. Like before, goal2a omits some
of the conditions in goal2a, but both goals should produce the same plan.
""")
state2 = pyhop2.State('state2')
state2.pos = {'a': 'c', 'b': 'd', 'c': 'table', 'd': 'table'}
state2.clear = {'a': True, 'c': False, 'b': True, 'd': False}
state2.holding = {'hand': False}
state2.display('Initial state is')
goal2a = pyhop2.Multigoal('goal2a')
goal2a.pos = {'b': 'c', 'a': 'd', 'c': 'table', 'd': 'table'}
goal2a.clear = {'a': True, 'c': False, 'b': True, 'd': False}
goal2a.holding = {'hand': False}
goal2a.display()
goal2b = pyhop2.Multigoal('goal2b')
goal2b.pos = {'b': 'c', 'a': 'd'}
goal2b.display()
### goal2b omits some of the conditions of goal2a,
### but those conditions will need to be achieved anyway.
expected = [('unstack', 'a', 'c'), ('putdown', 'a'), ('unstack', 'b', 'd'),
('stack', 'b', 'c'), ('pickup', 'a'), ('stack', 'a', 'd')]
plan1 = pyhop2.find_plan(state2, [goal2a], verbose=1)
check_result(plan1, expected)
plan2 = pyhop2.find_plan(state2, [goal2b], verbose=1)
check_result(plan2, expected)
pause()
print(
"\nRun find_plan on problem bw_large_d from the SHOP distribution:\n")
state3 = pyhop2.State('state3')
state3.pos = {
1: 12,
12: 13,
13: 'table',
11: 10,
10: 5,
5: 4,
4: 14,
14: 15,
15: 'table',
9: 8,
8: 7,
7: 6,
6: 'table',
19: 18,
18: 17,
17: 16,
16: 3,
3: 2,
2: 'table'
}
state3.clear = {x: False for x in range(1, 20)}
state3.clear.update({1: True, 11: True, 9: True, 19: True})
state3.holding = {'hand': False}
state3.display('Initial state is')
goal3 = pyhop2.Multigoal('goal3')
goal3.pos = {
15: 13,
13: 8,
8: 9,
9: 4,
4: 'table',
12: 2,
2: 3,
3: 16,
16: 11,
11: 7,
7: 6,
6: 'table'
}
goal3.clear = {17: True, 15: True, 12: True}
goal3.display()
expected = [('unstack', 1, 12), ('putdown', 1), ('unstack', 19, 18),
('putdown', 19), ('unstack', 18, 17), ('putdown', 18),
('unstack', 17, 16), ('putdown', 17), ('unstack', 9, 8),
('putdown', 9), ('unstack', 8, 7), ('putdown', 8),
('unstack', 11, 10), ('stack', 11, 7), ('unstack', 10, 5),
('putdown', 10), ('unstack', 5, 4), ('putdown', 5),
('unstack', 4, 14), ('putdown', 4), ('pickup', 9),
('stack', 9, 4), ('pickup', 8), ('stack', 8, 9),
('unstack', 14, 15), ('putdown', 14), ('unstack', 16, 3),
('stack', 16, 11), ('unstack', 3, 2), ('stack', 3, 16),
('pickup', 2), ('stack', 2, 3), ('unstack', 12, 13),
('stack', 12, 2), ('pickup', 13), ('stack', 13, 8),
('pickup', 15), ('stack', 15, 13)]
plan = pyhop2.find_plan(state3, [goal3], verbose=1)
check_result(plan, expected)
pause()
print("""
Call run_lazy_lookahead on the following problem, with verbose=1:
""")
state2.display(heading='Initial state is')
goal2b.display(heading='Goal is')
new_state = pyhop2.run_lazy_lookahead(state2, [goal2b], verbose=1)
pause()
print(
"The goal should now be satisfied, so the planner should return an empty plan:\n"
)
plan = pyhop2.find_plan(new_state, [goal2b], verbose=1)
check_result(plan, [])
print("No more examples")