def solve_pddlstream(focused=True, planner='max-astar', unit_costs=False):
problem = get_problem()
print(problem.constant_map)
print(problem.init)
print(problem.goal)
stream_info = {
't-ge': StreamInfo(eager=True),
'withdraw': StreamInfo(opt_gen_fn=PartialInputs(unique=True)),
}
with Profiler(field='cumtime'):
if focused:
solution = solve_focused(problem,
stream_info=stream_info,
planner=planner,
unit_costs=unit_costs,
initial_complexity=3,
clean=False,
debug=True,
verbose=True)
else:
solution = solve_incremental(problem,
planner=planner,
unit_costs=unit_costs,
clean=False,
debug=False,
verbose=True)
print_solution(solution)
plan, cost, certificate = solution
print('Certificate:', certificate.preimage_facts)
def main(focused=False, deterministic=False, unit_costs=True):
np.set_printoptions(precision=2)
if deterministic:
seed = 0
np.random.seed(seed)
print('Seed:', get_random_seed())
problem_fn = get_tight_problem # get_tight_problem | get_blocked_problem
tamp_problem = problem_fn()
print(tamp_problem)
stream_info = {
#'test-region': StreamInfo(eager=True, p_success=0), # bound_fn is None
#'plan-motion': StreamInfo(p_success=1), # bound_fn is None
#'trajcollision': StreamInfo(p_success=1), # bound_fn is None
#'cfree': StreamInfo(eager=True),
}
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
pr = cProfile.Profile()
pr.enable()
if focused:
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
max_time=10,
max_cost=INF,
debug=False,
effort_weight=None,
unit_costs=unit_costs,
postprocess=False,
visualize=False)
else:
solution = solve_incremental(pddlstream_problem,
layers=1,
unit_costs=unit_costs,
verbose=False)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS))
viewer = ContinuousTMPViewer(tamp_problem.regions, title='Continuous TAMP')
state = tamp_problem.initial
print()
print(state)
draw_state(viewer, state, colors)
for i, (action, args) in enumerate(plan):
user_input('Continue?')
print(i, action, args)
s2 = args[-1]
state = TAMPState(
s2[R], s2[H],
{b: s2[b]
for b in state.block_poses if s2[b] is not None})
print(state)
draw_state(viewer, state, colors)
user_input('Finish?')
def main():
parser = argparse.ArgumentParser()
#parser.add_argument('-p', '--problem', default='blocked', help='The name of the problem to solve')
parser.add_argument('-a',
'--algorithm',
default='focused',
help='Specifies the algorithm')
args = parser.parse_args()
print('Arguments:', args)
problem_fn = get_problem1 # get_problem1 | get_problem2
pddlstream_problem = problem_fn()
print('Init:', pddlstream_problem.init)
print('Goal:', pddlstream_problem.goal)
info = {
# Intentionally, misleading the stream
'increment': StreamInfo(p_success=0.01, overhead=1),
'decrement': StreamInfo(p_success=1, overhead=1),
}
if args.algorithm == 'focused':
solution = solve_focused(pddlstream_problem,
stream_info=info,
planner='max-astar',
effort_weight=1)
elif args.algorithm == 'incremental':
solution = solve_incremental(pddlstream_problem)
else:
raise ValueError(args.algorithm)
print_solution(solution)
def main(focused=True, unit_costs=False):
problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem
tamp_problem = problem_fn()
print(tamp_problem)
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=unit_costs)
else:
#solution = solve_exhaustive(pddlstream_problem, unit_costs=unit_costs)
solution = solve_incremental(pddlstream_problem, unit_costs=unit_costs)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
return
print(evaluations)
colors = dict(zip(tamp_problem.initial.block_poses, COLORS))
viewer = DiscreteTAMPViewer(1, len(tamp_problem.poses), title='Initial')
state = tamp_problem.initial
print(state)
draw_state(viewer, state, colors)
for action in plan:
user_input('Continue?')
state = apply_action(state, action)
print(state)
draw_state(viewer, state, colors)
user_input('Finish?')
def solve_pddlstream_satisfaction(stream_pddl,
stream_map,
init,
constraints,
incremental=False,
**kwargs):
# TODO: prune set of streams based on constraints
domain, goal = planning_from_satisfaction(init, constraints)
constant_map = {}
problem = PDDLProblem(domain, constant_map, stream_pddl, stream_map, init,
goal)
if incremental:
plan, cost, facts = solve_incremental(problem, **kwargs)
else:
plan, cost, facts = solve_focused(problem, **kwargs)
if plan is None:
return None, cost, facts
assert len(plan) == len(domain.actions)
bindings = {}
for action, (name, args) in safe_zip(domain.actions, plan):
assert action.name == name
for param, arg in safe_zip(action.parameters, args):
name = param.name
assert bindings.get(name, arg) is arg
bindings[name] = arg
return bindings, cost, facts
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-a', '--attachments', action='store_true')
parser.add_argument('-o', '--optimal', action='store_true', help='Runs in an anytime mode')
tamp_problem, args = initialize(parser)
stream_info = {
't-region': StreamInfo(eager=False, p_success=0),
'distance': FunctionInfo(opt_fn=lambda q1, q2: MOVE_COST),
}
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
dump_pddlstream(pddlstream_problem)
success_cost = 0 if args.optimal else INF
planner = 'max-astar'
#planner = 'ff-wastar1'
with Profiler():
if args.attachments:
solution = solve_incremental(pddlstream_problem, planner='ff-wastar1', max_time=args.max_time, verbose=True)
else:
solution = solve_focused(pddlstream_problem, stream_info=stream_info,
planner=planner, max_planner_time=10, debug=False,
max_time=args.max_time, max_iterations=INF, verbose=True,
unit_costs=args.unit, success_cost=success_cost,
unit_efforts=False, effort_weight=0,
max_skeletons=None, bind=True,
visualize=args.visualize)
print_solution(solution)
plan, cost, evaluations = solution
step_plan(tamp_problem, plan)
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-a',
'--algorithm',
default='incremental',
help='Specifies the algorithm')
args = parser.parse_args()
print('Arguments:', args)
problem_fn = get_problem # get_problem1 | get_problem2
pddlstream_problem = problem_fn()
print('Init:', pddlstream_problem.init)
print('Goal:', pddlstream_problem.goal)
info = {
# Intentionally, misleading the stream
'increment': StreamInfo(p_success=0.01, overhead=1),
'decrement': StreamInfo(p_success=1, overhead=1),
}
if args.algorithm == 'focused':
solution = solve_focused(pddlstream_problem,
stream_info=info,
planner='max-astar',
effort_weight=1)
elif args.algorithm == 'incremental':
solution = solve_incremental(
pddlstream_problem, #success_cost=0., max_iterations=3, max_time=5,
debug=False,
verbose=True)
else:
raise ValueError(args.algorithm)
print_solution(solution)
def main(planner='max-astar', unit_costs=True, defer=False):
parser = argparse.ArgumentParser()
parser.add_argument('-a', '--algorithm', default='focused', help='Specifies the algorithm')
args = parser.parse_args()
print('Arguments:', args)
pddlstream_problem = pddlstream_from_belief()
_, _, _, _, init, goal = pddlstream_problem
print('Init:', sorted(init, key=lambda f: f[0]))
print('Goal:', goal)
stream_info = {
'motion': StreamInfo(defer_fn=defer_shared if defer else never_defer),
}
replan_actions = set()
#replan_actions = {'phone'}
pr = cProfile.Profile()
pr.enable()
if args.algorithm == 'focused':
solution = solve_focused(pddlstream_problem, stream_info=stream_info, replan_actions=replan_actions,
planner=planner, unit_costs=unit_costs)
elif args.algorithm == 'incremental':
solution = solve_incremental(pddlstream_problem, planner=planner, unit_costs=unit_costs)
else:
raise NotImplementedError(args.algorithm)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(5)
print_solution(solution)
def main(deterministic=False, observable=False, collisions=True, focused=True, factor=True):
# TODO: global search over the state
belief_problem = get_belief_problem(deterministic, observable)
pddlstream_problem = to_pddlstream(belief_problem, collisions)
pr = cProfile.Profile()
pr.enable()
planner = 'ff-wastar1'
if focused:
stream_info = {
'GE': StreamInfo(from_test(ge_fn), eager=False),
'prob-after-move': StreamInfo(from_fn(get_opt_move_fn(factor=factor))),
'MoveCost': FunctionInfo(move_cost_fn),
'prob-after-look': StreamInfo(from_fn(get_opt_obs_fn(factor=factor))),
'LookCost': FunctionInfo(get_look_cost_fn(p_look_fp=0, p_look_fn=0)),
}
solution = solve_focused(pddlstream_problem, stream_info=stream_info, planner=planner, debug=False,
max_cost=0, unit_costs=False, max_time=30)
else:
solution = solve_incremental(pddlstream_problem, planner=planner, debug=True,
max_cost=MAX_COST, unit_costs=False, max_time=30)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
print_solution(solution)
plan, cost, init = solution
print('Real cost:', float(cost)/SCALE_COST)
def solve_pddlstream(focused=False):
pddlstream_problem = get_problem()
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=True)
else:
solution = solve_incremental(pddlstream_problem, unit_costs=True, planner='cerberus', debug=False)
print_solution(solution)
def solve_pddlstream(focused=True):
pddlstream_problem = get_problem()
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=True)
else:
#solution = solve_exhaustive(pddlstream_problem, unit_costs=True)
solution = solve_incremental(pddlstream_problem, unit_costs=True)
print_solution(solution)
def solve_pddlstream(focused=True):
problem_fn = get_problem1 # get_problem1 | get_problem2
pddlstream_problem = problem_fn()
print('Init:', pddlstream_problem.init)
print('Goal:', pddlstream_problem.goal)
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=True)
else:
solution = solve_incremental(pddlstream_problem, unit_costs=True)
print_solution(solution)
def solve_pddlstream_satisfaction(stream_pddl, stream_map, init, constraints, incremental=False, **kwargs):
# TODO: prune set of streams based on constraints
# TODO: investigate constraint satisfaction techniques for search instead
# TODO: optimistic objects based on free parameters that prevent cycles
# TODO: disallow creation of new parameters / certifying new facts
problem = pddl_from_csp(stream_pddl, stream_map, init, constraints)
if incremental:
plan, cost, facts = solve_incremental(problem, **kwargs)
else:
plan, cost, facts = solve_focused(problem, **kwargs)
bindings = bindings_from_plan(problem, plan)
return bindings, cost, facts
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-p', '--problem', default='mirror', help='The name of the problem to solve')
parser.add_argument('-a', '--algorithm', default='incremental', help='Specifies the algorithm')
parser.add_argument('-c', '--cfree', action='store_true', help='Disables collisions')
parser.add_argument('-d', '--deterministic', action='store_true', help='Uses a deterministic sampler')
parser.add_argument('-g', '--gurobi', action='store_true', help='Uses gurobi')
parser.add_argument('-n', '--number', default=1, type=int, help='The number of blocks')
parser.add_argument('-o', '--optimal', action='store_true', help='Runs in an anytime mode')
parser.add_argument('-s', '--skeleton', action='store_true', help='Enforces skeleton plan constraints')
parser.add_argument('-t', '--max_time', default=30, type=int, help='The max time')
parser.add_argument('-u', '--unit', action='store_true', help='Uses unit costs')
parser.add_argument('-v', '--visualize', action='store_true', help='Visualizes graphs')
args = parser.parse_args()
print('Arguments:', args)
np.set_printoptions(precision=2)
if args.deterministic:
random.seed(seed=0)
np.random.seed(seed=0)
print('Random seed:', get_random_seed())
problem_from_name = {fn.__name__: fn for fn in PROBLEMS}
if args.problem not in problem_from_name:
raise ValueError(args.problem)
print('Problem:', args.problem)
problem_fn = problem_from_name[args.problem]
tamp_problem = problem_fn(args.number)
print(tamp_problem)
pddlstream_problem = pddlstream_from_tamp(tamp_problem, collisions=not args.cfree,
use_stream=not args.gurobi, use_optimizer=args.gurobi)
print('Constants:', str_from_object(pddlstream_problem.constant_map))
print('Initial:', sorted_str_from_list(pddlstream_problem.init))
print('Goal:', str_from_object(pddlstream_problem.goal))
success_cost = 0 if args.optimal else INF
planner = 'max-astar'
#planner = 'ff-wastar1'
with Profiler(field='cumtime', num=20):
if args.algorithm == 'incremental':
solution = solve_incremental(pddlstream_problem,
complexity_step=1, planner=planner,
unit_costs=args.unit, success_cost=success_cost,
max_time=args.max_time, verbose=False)
else:
raise ValueError(args.algorithm)
print_solution(solution)
plan, cost, evaluations = solution
if plan is not None:
display_plan(tamp_problem, plan)
def main(max_time=20):
"""
Creates and solves the 2D motion planning problem.
"""
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
obstacles = [create_box((.5, .5), (.2, .2))]
regions = {
'env': create_box((.5, .5), (1, 1)),
'green': create_box((.8, .8), (.4, .4)),
}
goal = 'green'
if goal not in regions:
goal = ARRAY([1, 1])
max_distance = 0.25 # 0.2 | 0.25 | 0.5 | 1.0
problem, samples, roadmap = create_problem(goal,
obstacles,
regions,
max_distance=max_distance)
print('Initial:', str_from_object(problem.init))
print('Goal:', str_from_object(problem.goal))
constraints = PlanConstraints(max_cost=1.25) # max_cost=INF)
with Profiler(field='tottime', num=10):
solution = solve_incremental(problem,
constraints=constraints,
unit_costs=args.unit,
success_cost=0,
max_time=max_time,
verbose=False)
print_solution(solution)
plan, cost, evaluations = solution
#viewer = draw_environment(obstacles, regions)
#for sample in samples:
# viewer.draw_point(sample)
#user_input('Continue?')
# TODO: use the same viewer here
draw_roadmap(roadmap, obstacles, regions) # TODO: do this in realtime
user_input('Continue?')
if plan is None:
return
segments = [args for name, args in plan]
draw_solution(segments, obstacles, regions)
user_input('Finish?')
def main(max_time=20):
"""
Creates and solves the 2D motion planning problem.
"""
obstacles = [create_box((.5, .5), (.2, .2))]
regions = {
'env': create_box((.5, .5), (1, 1)),
'green': create_box((.8, .8), (.4, .4)),
}
goal = 'green'
if goal not in regions:
goal = array([1, 1])
max_distance = 0.25 # 0.2 | 0.25 | 0.5 | 1.0
problem, samples, roadmap = create_problem(goal,
obstacles,
regions,
max_distance=max_distance)
print('Initial:', str_from_object(problem.init))
print('Goal:', str_from_object(problem.goal))
pr = cProfile.Profile()
pr.enable()
solution = solve_incremental(problem,
unit_costs=False,
max_cost=0,
max_time=max_time,
verbose=False)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
print_solution(solution)
plan, cost, evaluations = solution
#viewer = draw_environment(obstacles, regions)
#for sample in samples:
# viewer.draw_point(sample)
#user_input('Continue?')
# TODO: use the same viewer here
draw_roadmap(roadmap, obstacles, regions) # TODO: do this in realtime
user_input('Continue?')
if plan is None:
return
segments = [args for name, args in plan]
draw_solution(segments, obstacles, regions)
user_input('Finish?')
def main(focused=True, unit_costs=False):
problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem
tamp_problem = problem_fn()
print(tamp_problem)
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=unit_costs)
else:
solution = solve_incremental(pddlstream_problem, unit_costs=unit_costs)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
return
apply_plan(tamp_problem, plan)
def main():
parser = argparse.ArgumentParser()
tamp_problem, args = initialize(parser)
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
dump_pddlstream(pddlstream_problem)
with Profiler():
solution = solve_incremental(pddlstream_problem,
planner='ff-wastar1',
max_time=args.max_time,
verbose=False)
print_solution(solution)
plan, cost, evaluations = solution
step_plan(tamp_problem, plan)
def main(focused=True):
# TODO: maybe load problems as a domain explicitly
pddlstream_problem = pddlstream_from_belief()
_, _, _, _, init, goal = pddlstream_problem
print('Init:', sorted(init, key=lambda f: f[0]))
print('Goal:', goal)
pr = cProfile.Profile()
pr.enable()
if focused:
solution = solve_focused(pddlstream_problem, unit_costs=False)
else:
#solution = solve_exhaustive(pddlstream_problem, unit_costs=False)
solution = solve_incremental(pddlstream_problem, unit_costs=False)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(5)
print_solution(solution)
def main(deterministic=False, unit_costs=True):
np.set_printoptions(precision=2)
if deterministic:
seed = 0
np.random.seed(seed)
print('Seed:', get_random_seed())
problem_fn = tight # get_tight_problem | get_blocked_problem
tamp_problem = problem_fn(n_blocks=1, n_goals=1, n_robots=1)
print(tamp_problem)
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
with Profiler():
solution = solve_incremental(pddlstream_problem, complexity_step=1, max_time=30,
unit_costs=unit_costs, verbose=False)
print_solution(solution)
plan, cost, evaluations = solution
step_plan(tamp_problem, plan)
def main(max_time=20):
"""
Creates and solves the 2D motion planning problem.
"""
obstacles = [create_box((.5, .5), (.2, .2))]
regions = {
'env': create_box((.5, .5), (1, 1)),
#'goal': create_box((.8, .8), (.4, .4)),
}
goal = np.array([1, 1])
#goal = 'goal'
max_distance = 0.25 # 0.2 | 0.25 | 0.5 | 1.0
problem, roadmap = create_problem(goal,
obstacles,
regions,
max_distance=max_distance)
pr = cProfile.Profile()
pr.enable()
solution = solve_incremental(problem,
unit_costs=False,
max_cost=0,
max_time=max_time,
verbose=False)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
print_solution(solution)
plan, cost, evaluations = solution
print('Plan:', plan)
if plan is None:
return
# TODO: use the same viewer here
draw_roadmap(roadmap, obstacles, regions) # TODO: do this in realtime
user_input('Continue?')
segments = [args for name, args in plan]
draw_solution(segments, obstacles, regions)
user_input('Finish?')
def main():
parser = argparse.ArgumentParser()
#parser.add_argument('-p', '--problem', default='blocked', help='The name of the problem to solve')
parser.add_argument('-a',
'--algorithm',
default='focused',
help='Specifies the algorithm')
parser.add_argument('-u',
'--unit',
action='store_true',
help='Uses unit costs')
args = parser.parse_args()
print('Arguments:', args)
problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem
tamp_problem = problem_fn()
print(tamp_problem)
stream_info = {
'test-cfree': StreamInfo(negate=True),
}
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
if args.algorithm == 'focused':
#solution = solve_execution(pddlstream_problem, unit_costs=unit_costs, stream_info=stream_info)
solution = solve_focused(pddlstream_problem,
unit_costs=args.unit,
stream_info=stream_info,
debug=False)
elif args.algorithm == 'current':
# Should fail to find a solution
solution = solve_current(pddlstream_problem, unit_costs=args.unit)
elif args.algorithm == 'exhaustive':
solution = solve_exhaustive(pddlstream_problem, unit_costs=args.unit)
elif args.algorithm == 'incremental':
solution = solve_incremental(pddlstream_problem, unit_costs=args.unit)
else:
raise ValueError(args.algorithm)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
return
apply_plan(tamp_problem, plan)
def main(focused=True, unit_costs=False):
problem_fn = get_shift_one_problem # get_shift_one_problem | get_shift_all_problem
tamp_problem = problem_fn()
print(tamp_problem)
stream_info = {
# TODO: be careful when negate=False. Might produce a colliding solution
'test-cfree': StreamInfo(negate=True),
}
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
if focused:
solution = solve_focused(pddlstream_problem, stream_info=stream_info, unit_costs=unit_costs)
else:
solution = solve_incremental(pddlstream_problem, unit_costs=unit_costs)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
return
apply_plan(tamp_problem, plan)
def solve_problem(seed=None, simulate=False):
p.connect(p.DIRECT)
np.random.seed(seed)
np_state = np.random.get_state()
problem = get_problem()
pr = cProfile.Profile()
pr.enable()
solution = solve_incremental(problem, verbose=False)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10) # cumtime | tottime
plan, cost, evaluations = solution
print_solution(solution)
p.disconnect()
if simulate:
p.connect(p.GUI)
np.random.set_state(np_state)
table_aabb, robot, region, objects, poses, init_extend = load_world()
execute_plan(plan, (table_aabb, objects))
step_simulation(steps=1000, simulate=True)
p.disconnect()
def main():
uniform_rooms = UniformDist(['room0', OTHER])
#uniform_tables = UniformDist(['table0', 'table1'])
#uniform_tables = UniformDist(['table0', OTHER])
uniform_tables = UniformDist(['table0', 'table1', OTHER])
#initial_belief = get_room_belief(uniform_rooms, uniform_tables, 1.0)
initial_belief = get_room_belief(uniform_rooms, uniform_tables, 0.2)
#initial_belief = get_table_belief(uniform_tables, 1.0)
#initial_belief = get_table_belief(uniform_tables, 0.2)
#initial_belief = get_item_belief()
pddlstream_problem = pddlstream_from_belief(initial_belief)
_, _, _, _, init, goal = pddlstream_problem
print(sorted(init))
print(goal)
pr = cProfile.Profile()
pr.enable()
solution = solve_incremental(pddlstream_problem, unit_costs=False)
print_solution(solution)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
def main(deterministic=False, unit_costs=True):
np.set_printoptions(precision=2)
if deterministic:
seed = 0
np.random.seed(seed)
print('Seed:', get_random_seed())
problem_fn = tight # get_tight_problem | get_blocked_problem
tamp_problem = problem_fn(n_blocks=2, n_goals=2, n_robots=1)
print(tamp_problem)
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
with Profiler():
solution = solve_incremental(pddlstream_problem, complexity_step=1, max_time=30, planner='dijkstra',
unit_costs=unit_costs, verbose=False)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
return
# TODO: might still be a planning bug
viewer = ContinuousTMPViewer(SUCTION_HEIGHT, tamp_problem.regions, title='Continuous TAMP')
conf = conf_from_state(tamp_problem.initial)
print()
print(conf)
draw_conf(viewer, tamp_problem, conf)
user_input('Start?')
for i, (action, args) in enumerate(plan):
print(i, action, args)
if action == 'switch':
continue
traj = args[-1]
for conf in traj[1:]:
print(conf)
draw_conf(viewer, tamp_problem, conf)
user_input('Continue?')
user_input('Finish?')
def main(display=True, teleport=False):
parser = argparse.ArgumentParser()
parser.add_argument('-algorithm', default='incremental', help='Specifies the algorithm')
parser.add_argument('-cfree', action='store_true', help='Disables collisions')
parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler')
parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode')
parser.add_argument('-t', '--max_time', default=5*60, type=int, help='The max time')
parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning')
args = parser.parse_args()
print(args)
#problem_fn_from_name = {fn.__name__: fn for fn in PROBLEMS}
#if args.problem not in problem_fn_from_name:
# raise ValueError(args.problem)
#problem_fn = problem_fn_from_name[args.problem]
connect(use_gui=args.viewer)
with HideOutput():
problem = problem_fn(collisions=not args.cfree)
saver = WorldSaver()
draw_base_limits(problem.limits, color=RED)
pddlstream, edges = pddlstream_from_problem(problem, teleport=teleport)
_, constant_map, _, stream_map, init, goal = pddlstream
print('Constants:', constant_map)
print('Init:', init)
print('Goal:', goal)
success_cost = 0 if args.optimal else INF
max_planner_time = 10
stream_info = {
'compute-motion': StreamInfo(eager=True, p_success=0),
'ConfConfCollision': FunctionInfo(p_success=1, overhead=0.1),
'TrajConfCollision': FunctionInfo(p_success=1, overhead=1),
'TrajTrajCollision': FunctionInfo(p_success=1, overhead=10),
'TrajDistance': FunctionInfo(eager=True), # Need to eagerly evaluate otherwise 0 duration (failure)
}
pr = cProfile.Profile()
pr.enable()
with LockRenderer(False):
if args.algorithm == 'incremental':
solution = solve_incremental(pddlstream,
max_planner_time=max_planner_time,
success_cost=success_cost, max_time=args.max_time,
start_complexity=INF,
verbose=True, debug=True)
elif args.algorithm == 'focused':
solution = solve_focused(pddlstream, stream_info=stream_info,
max_planner_time=max_planner_time,
success_cost=success_cost, max_time=args.max_time,
max_skeletons=None, bind=True, max_failures=INF,
verbose=True, debug=True)
else:
raise ValueError(args.algorithm)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime
if plan is None:
wait_for_user()
return
if (not display) or (plan is None):
disconnect()
return
if not args.viewer:
disconnect()
connect(use_gui=True)
with LockRenderer():
with HideOutput():
problem_fn() # TODO: way of doing this without reloading?
saver.restore() # Assumes bodies are ordered the same way
draw_edges(edges)
state = BeliefState(problem)
wait_for_user()
#time_step = None if teleport else 0.01
#with VideoSaver('video.mp4'):
display_plan(problem, state, plan)
wait_for_user()
disconnect()
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-a',
'--algorithm',
default='focused',
help='Specifies the algorithm')
parser.add_argument('-g',
'--gurobi',
action='store_true',
help='Uses gurobi')
parser.add_argument('-o',
'--optimal',
action='store_true',
help='Runs in an anytime mode')
parser.add_argument('-s',
'--skeleton',
action='store_true',
help='Enforces skeleton plan constraints')
# TODO: test if placed in the same region
defer_fn = defer_shared # never_defer | defer_unique | defer_shared
tamp_problem, args = initialize(parser)
stream_info = {
's-region':
StreamInfo(defer_fn=defer_fn),
's-grasp':
StreamInfo(defer_fn=defer_fn),
's-ik':
StreamInfo(defer_fn=get_defer_all_unbound(
inputs='?g')), # defer_fn | defer_unbound
's-motion':
StreamInfo(defer_fn=get_defer_any_unbound()),
't-cfree':
StreamInfo(defer_fn=get_defer_any_unbound(), eager=False,
negate=True), # defer_fn | defer_unbound
't-region':
StreamInfo(eager=False, p_success=0), # bound_fn is None
'dist':
FunctionInfo(defer_fn=get_defer_any_unbound(),
opt_fn=lambda q1, q2: MOVE_COST),
'gurobi-cfree':
StreamInfo(eager=False, negate=True),
#'gurobi': OptimizerInfo(p_success=0),
#'rrt': OptimizerInfo(p_success=0),
}
hierarchy = [
#ABSTRIPSLayer(pos_pre=['atconf']), #, horizon=1),
]
skeletons = [TIGHT_SKELETON] if args.skeleton else None
assert implies(args.skeleton, args.problem == 'tight')
max_cost = INF # 8*MOVE_COST
constraints = PlanConstraints(
skeletons=skeletons,
#skeletons=[],
#skeletons=[skeleton, []],
exact=True,
max_cost=max_cost)
#replan_actions = set()
replan_actions = {'move', 'pick', 'place'}
pddlstream_problem = pddlstream_from_tamp(tamp_problem,
collisions=not args.cfree,
use_stream=not args.gurobi,
use_optimizer=args.gurobi)
dump_pddlstream(pddlstream_problem)
pr = cProfile.Profile()
pr.enable()
success_cost = 0 if args.optimal else INF
planner = 'max-astar'
#planner = 'ff-wastar1'
if args.algorithm == 'focused':
solver = solve_focused # solve_focused | solve_serialized
solution = solver(
pddlstream_problem,
constraints=constraints,
stream_info=stream_info,
replan_actions=replan_actions,
planner=planner,
max_planner_time=10,
hierarchy=hierarchy,
debug=False,
max_time=args.max_time,
max_iterations=INF,
verbose=True,
unit_costs=args.unit,
success_cost=success_cost,
unit_efforts=True,
effort_weight=1,
search_sample_ratio=1,
#max_skeletons=None, bind=True,
visualize=args.visualize)
elif args.algorithm == 'incremental':
solution = solve_incremental(pddlstream_problem,
constraints=constraints,
complexity_step=2,
planner=planner,
hierarchy=hierarchy,
unit_costs=args.unit,
success_cost=success_cost,
max_time=args.max_time,
verbose=False)
else:
raise ValueError(args.algorithm)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('cumtime').print_stats(20)
if plan is not None:
display_plan(tamp_problem, retime_plan(plan))
def main(focused=True,
deterministic=True,
unit_costs=False,
use_synthesizers=False):
np.set_printoptions(precision=2)
if deterministic:
seed = 0
np.random.seed(seed)
print('Seed:', get_random_seed())
if use_synthesizers and not has_gurobi():
use_synthesizers = False
print(
'Warning! use_synthesizers=True requires gurobipy. Setting use_synthesizers=False.'
)
print('Focused: {} | Costs: {} | Synthesizers: {}'.format(
focused, not unit_costs, use_synthesizers))
problem_fn = get_blocked_problem # get_tight_problem | get_blocked_problem
tamp_problem = problem_fn()
print(tamp_problem)
action_info = {
#'move': ActionInfo(terminal=True),
#'pick': ActionInfo(terminal=True),
#'place': ActionInfo(terminal=True),
}
stream_info = {
't-region': StreamInfo(eager=True, p_success=0), # bound_fn is None
't-cfree': StreamInfo(eager=False, negate=True),
#'distance': FunctionInfo(opt_fn=lambda *args: 1),
#'gurobi': OptimizerInfo(p_success=0),
#'rrt': OptimizerInfo(p_success=0),
}
hierarchy = [
#ABSTRIPSLayer(pos_pre=['atconf']), #, horizon=1),
]
synthesizers = [
#StreamSynthesizer('cfree-motion', {'s-motion': 1, 'trajcollision': 0},
# gen_fn=from_fn(cfree_motion_fn)),
StreamSynthesizer('optimize', {
's-region': 1,
's-ik': 1,
'posecollision': 0,
't-cfree': 0,
'distance': 0
},
gen_fn=from_fn(get_optimize_fn(
tamp_problem.regions))),
] if use_synthesizers else []
pddlstream_problem = pddlstream_from_tamp(tamp_problem)
print('Initial:', str_from_object(pddlstream_problem.init))
print('Goal:', str_from_object(pddlstream_problem.goal))
pr = cProfile.Profile()
pr.enable()
if focused:
solution = solve_focused(
pddlstream_problem,
action_info=action_info,
stream_info=stream_info,
planner='ff-wastar1',
max_planner_time=10,
synthesizers=synthesizers,
verbose=True,
max_time=300,
max_cost=INF,
debug=False,
hierarchy=hierarchy,
effort_weight=1,
search_sampling_ratio=0, # TODO: run without to see difference
unit_costs=unit_costs,
postprocess=False,
visualize=False)
else:
solution = solve_incremental(pddlstream_problem,
layers=1,
hierarchy=hierarchy,
unit_costs=unit_costs,
verbose=False)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is not None:
display_plan(tamp_problem, plan)
def main(teleport=False):
#parser = create_parser()
parser = argparse.ArgumentParser()
parser.add_argument('-algorithm', default='incremental', help='Specifies the algorithm')
parser.add_argument('-cfree', action='store_true', help='Disables collisions')
parser.add_argument('-deterministic', action='store_true', help='Uses a deterministic sampler')
parser.add_argument('-optimal', action='store_true', help='Runs in an anytime mode')
parser.add_argument('-t', '--max_time', default=5*60, type=int, help='The max time')
parser.add_argument('-enable', action='store_true', help='Enables rendering during planning')
parser.add_argument('-viewer', action='store_true', help='Enable the viewer and visualizes the plan')
args = parser.parse_args()
print('Arguments:', args)
connect(use_gui=args.viewer)
with HideOutput():
namo_problem = problem_fn(collisions=not args.cfree)
saver = WorldSaver()
draw_base_limits(namo_problem.limits, color=RED)
pddlstream_problem, edges = pddlstream_from_problem(namo_problem, teleport=teleport)
_, constant_map, _, stream_map, init, goal = pddlstream_problem
print('Constants:', constant_map)
print('Init:', init)
print('Goal:', goal)
stream_info = {
'compute-motion': StreamInfo(eager=True, p_success=0),
'ConfConfCollision': PredicateInfo(p_success=1, overhead=0.1),
'TrajConfCollision': PredicateInfo(p_success=1, overhead=1),
'TrajTrajCollision': PredicateInfo(p_success=1, overhead=10),
'TrajDistance': FunctionInfo(eager=True), # Need to eagerly evaluate otherwise 0 duration (failure)
}
success_cost = 0 if args.optimal else INF
max_planner_time = 10
with Profiler(field='tottime', num=25): # cumtime | tottime
with LockRenderer(lock=not args.enable):
# TODO: solution = solve_incremental(pddlstream_problem
if args.algorithm == 'incremental':
solution = solve_incremental(pddlstream_problem,
max_planner_time=max_planner_time,
success_cost=success_cost, max_time=args.max_time,
start_complexity=INF,
verbose=True, debug=True)
elif args.algorithm == 'focused':
solution = solve_focused(pddlstream_problem, stream_info=stream_info,
max_planner_time=max_planner_time,
success_cost=success_cost, max_time=args.max_time,
max_skeletons=None, bind=True, max_failures=INF,
verbose=True, debug=True)
else:
raise ValueError(args.algorithm)
print_solution(solution)
plan, cost, evaluations = solution
if (plan is None) or not has_gui():
disconnect()
return
saver.restore()
draw_edges(edges)
state = BeliefState(namo_problem)
wait_for_user('Begin?')
#time_step = None if teleport else 0.01
#with VideoSaver('video.mp4'):
display_plan(namo_problem, state, plan)
wait_for_user('Finish?')
disconnect()
