def main():
"""
Creates and solves the 2D motion planning problem.
"""
obstacles = [create_box((.5, .5), .2, .2)]
goal = create_box((.9, .9), .2, .2)
constraint_problem = create_problem(goal, obstacles)
print
print constraint_problem
stream_problem = constraint_to_stripstream(constraint_problem)
print
print stream_problem
search = get_fast_downward('astar') # dijkstra | astar
plan, _ = incremental_planner(stream_problem,
search=search,
frequency=25,
waves=False)
plan = convert_plan(plan)
print 'Plan:', plan
draw_solution(plan, goal, obstacles)
raw_input('Finish?')
def solve_tamp(env):
viewer = env.GetViewer() is not None
problem = PROBLEM(env)
robot, manipulator, base_manip, _ = initialize_openrave(env,
ARM,
min_delta=.01)
bodies = {obj: env.GetKinBody(obj) for obj in problem.object_names}
open_gripper(manipulator)
initial_conf = Conf(
robot.GetConfigurationValues()[manipulator.GetArmIndices()])
####################
fts_problem = get_problem(env, problem, robot, manipulator, base_manip,
bodies, initial_conf)
print
print fts_problem
stream_problem = constraint_to_stripstream(fts_problem)
print
print stream_problem
for stream in stream_problem.cond_streams:
print stream, stream.max_level
# TODO - why is this slower/less reliable than the other one (extra axioms, eager evaluation?)
if viewer: raw_input('Start?')
search_fn = get_fast_downward('eager', max_time=10, verbose=False)
#solve = lambda: incremental_planner(stream_problem, search=search_fn, frequency=1, waves=True, debug=False)
solve = lambda: simple_focused(stream_problem,
search=search_fn,
max_level=INF,
shared=False,
debug=False,
verbose=False)
env.Lock()
plan, universe = solve()
env.Unlock()
print SEPARATOR
plan = convert_plan(plan)
if plan is not None:
print 'Success'
for i, (action, args) in enumerate(plan):
print i + 1, action, args
else:
print 'Failure'
####################
if viewer and plan is not None:
print SEPARATOR
visualize_solution(env, problem, initial_conf, robot, manipulator,
bodies, plan)
raw_input('Finish?')
def main():
"""
Creates and solves the 1D task and motion planning FTSProblem problem.
"""
constraint_problem = create_problem()
print
print constraint_problem
stream_problem = constraint_to_stripstream(constraint_problem)
print
print stream_problem
# 'dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
search_fn = get_fast_downward('eager')
plan, _ = incremental_planner(stream_problem, search=search_fn)
print
print 'Plan:', convert_plan(plan)