def main(display=True, teleport=False):
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
parser.add_argument('-simulate', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=args.viewer)
robot, names, movable = load_world()
saved_world = WorldSaver()
#dump_world()
pddlstream_problem = pddlstream_from_problem(robot, movable=movable, teleport=teleport)
_, _, _, stream_map, init, goal = pddlstream_problem
synthesizers = [
StreamSynthesizer('safe-free-motion', {'plan-free-motion': 1, 'trajcollision': 0},
from_fn(get_free_motion_synth(robot, movable, teleport))),
StreamSynthesizer('safe-holding-motion', {'plan-holding-motion': 1, 'trajcollision': 0},
from_fn(get_holding_motion_synth(robot, movable, teleport))),
] if USE_SYNTHESIZERS else []
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
print('Synthesizers:', stream_map.keys())
print(names)
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem, synthesizers=synthesizers, success_cost=INF)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
if (not display) or (plan is None):
disconnect()
return
if not args.viewer: # TODO: how to reenable the viewer
disconnect()
connect(use_gui=True)
load_world()
else:
saved_world.restore()
command = postprocess_plan(plan)
if args.simulate:
user_input('Simulate?')
command.control()
else:
user_input('Execute?')
#command.step()
command.refine(num_steps=10).execute(time_step=0.001)
#wait_for_interrupt()
user_input('Finish?')
disconnect()
def main(display=True, simulate=False, teleport=False):
parser = argparse.ArgumentParser()
parser.add_argument('-viewer',
action='store_true',
help='enable the viewer while planning')
#parser.add_argument('-display', action='store_true', help='displays the solution')
args = parser.parse_args()
connect(use_gui=args.viewer)
problem_fn = cooking_problem
# holding_problem | stacking_problem | cleaning_problem | cooking_problem
# cleaning_button_problem | cooking_button_problem
with HideOutput():
problem = problem_fn()
state_id = save_state()
#saved_world = WorldSaver()
#dump_world()
pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport)
stream_info = {
'sample-pose': StreamInfo(PartialInputs('?r')),
'inverse-kinematics': StreamInfo(PartialInputs('?p')),
'plan-base-motion': StreamInfo(PartialInputs('?q1 ?q2')),
'MoveCost': FunctionInfo(opt_move_cost_fn),
}
synthesizers = [
StreamSynthesizer(
'safe-base-motion', {
'plan-base-motion': 1,
'TrajPoseCollision': 0,
'TrajGraspCollision': 0,
'TrajArmCollision': 0,
}, from_fn(get_base_motion_synth(problem, teleport))),
] if USE_SYNTHESIZERS else []
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
print('Synthesizers:', synthesizers)
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
synthesizers=synthesizers,
success_cost=INF)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
if plan is None:
return
if (not display) or (plan is None):
disconnect()
return
commands = post_process(problem, plan)
if args.viewer:
restore_state(state_id)
else:
disconnect()
connect(use_gui=True)
with HideOutput():
problem_fn() # TODO: way of doing this without reloading?
if simulate:
enable_gravity()
control_commands(commands)
else:
step_commands(commands, time_step=0.01)
user_input('Finish?')
disconnect()
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(use_synthesizers=False):
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('-c', '--cfree', action='store_true', help='Disables collisions')
parser.add_argument('-d', '--deterministic', action='store_true', help='Uses a deterministic sampler')
parser.add_argument('-u', '--unit', action='store_true', help='Uses unit costs')
parser.add_argument('-o', '--optimal', action='store_true', help='Runs in an anytime mode')
parser.add_argument('-t', '--max_time', default=20, type=int, help='The max time')
args = parser.parse_args()
print('Arguments:', args)
print('Synthesizers: {}'.format(use_synthesizers))
np.set_printoptions(precision=2)
if args.deterministic:
seed = 0
np.random.seed(seed)
print('Random 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.')
if args.problem not in PROBLEMS:
raise ValueError(args.problem)
print('Problem:', args.problem)
problem_fn = PROBLEMS[args.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 q1, q2: MOVE_COST),
#'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 []
skeleton = [
('move', ['?q0', WILD, '?q1']),
('pick', ['b0', '?p0', '?q1']),
('move', ['?q1', WILD, '?q2']),
('place', ['b0', '?p1', '?q2']),
]
constraints = PlanConstraints(#skeletons=None,
#skeletons=[],
#skeletons=[skeleton],
#skeletons=[skeleton, []],
#exact=False,
max_cost=INF)
pddlstream_problem = pddlstream_from_tamp(tamp_problem, collisions=not args.cfree)
print('Initial:', str_from_object(pddlstream_problem.init))
print('Goal:', str_from_object(pddlstream_problem.goal))
pr = cProfile.Profile()
pr.enable()
success_cost = 0 if args.optimal else INF
if args.algorithm == 'focused':
solution = solve_focused(pddlstream_problem, constraints=constraints,
action_info=action_info, stream_info=stream_info, synthesizers=synthesizers,
planner='ff-wastar1', 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=False, effort_weight=0,
search_sample_ratio=0,
#max_skeletons=None,
visualize=False)
elif args.algorithm == 'incremental':
solution = solve_incremental(pddlstream_problem, constraints=constraints,
complexity_step=2, 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('tottime').print_stats(10)
if plan is not None:
display_plan(tamp_problem, plan)