def main(display=True, teleport=False):
parser = argparse.ArgumentParser()
#parser.add_argument('-problem', default='rovers1', help='The name of the problem to solve')
parser.add_argument('-algorithm',
default='focused',
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=120,
type=int,
help='The max time')
parser.add_argument('-unit', action='store_true', help='Uses unit costs')
parser.add_argument('-simulate',
action='store_true',
help='Simulates the system')
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_problem = pddlstream_from_problem(problem, teleport=teleport)
stream_info = {
'test-cfree-conf-pose': StreamInfo(negate=True, p_success=1e-2),
'test-cfree-traj-pose': StreamInfo(negate=True, p_success=1e-1),
'compute-motion': StreamInfo(eager=True, p_success=0),
'test-reachable': StreamInfo(eager=True),
'Distance': FunctionInfo(eager=True),
}
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
success_cost = 0 if args.optimal else INF
planner = 'ff-wastar1'
search_sample_ratio = 0
max_planner_time = 10
pr = cProfile.Profile()
pr.enable()
with LockRenderer(True):
if args.algorithm == 'focused':
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
planner=planner,
max_planner_time=max_planner_time,
debug=False,
unit_costs=args.unit,
success_cost=success_cost,
max_time=args.max_time,
verbose=True,
unit_efforts=True,
effort_weight=1,
bind=True,
max_skeletons=None,
search_sample_ratio=search_sample_ratio)
elif args.algorithm == 'incremental':
solution = solve_incremental(pddlstream_problem,
planner=planner,
max_planner_time=max_planner_time,
unit_costs=args.unit,
success_cost=success_cost,
max_time=args.max_time,
verbose=True)
else:
raise ValueError(args.algorithm)
saver.restore()
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
with LockRenderer():
commands = post_process(problem, plan, teleport=teleport)
saver.restore() # Assumes bodies are ordered the same way
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
wait_for_user()
if args.simulate:
control_commands(commands)
else:
time_step = None if teleport else 0.01
apply_commands(BeliefState(problem), commands, time_step=time_step)
wait_for_user()
disconnect()
def post_process(state,
plan,
replan_obs=True,
replan_base=False,
look_move=True):
problem = state.task
if plan is None:
return None
# TODO: refine actions
robot = problem.robot
commands = []
uncertain_base = False
expecting_obs = False
for i, (name, args) in enumerate(plan):
if replan_obs and expecting_obs:
break
saved_world = WorldSaver() # StateSaver()
if name == 'move_base':
t = args[-1]
# TODO: look at the trajectory (endpoint or path) to ensure fine
# TODO: I should probably move base and keep looking at the path
# TODO: I could keep updating the head goal as the base moves along the path
if look_move:
new_commands = [move_look_trajectory(t)]
#new_commands = [inspect_trajectory(t), t]
else:
new_commands = [t]
if replan_base:
uncertain_base = True
elif name == 'pick':
if uncertain_base:
break
a, b, p, g, _, t = args
attach = Attach(robot, a, g, b)
new_commands = [t, attach, t.reverse()]
elif name == 'place':
if uncertain_base:
break
a, b, p, g, _, t = args
detach = Detach(robot, a, b)
new_commands = [t, detach, t.reverse()]
elif name == 'scan':
o, p, bq, hq, ht = args
ht0 = plan_head_traj(robot, hq.values)
new_commands = [ht0]
if o in problem.rooms:
attach, detach = get_cone_commands(robot)
new_commands += [attach, ht, ScanRoom(robot, o), detach]
else:
new_commands += [ht, Scan(robot, o)]
#with BodySaver(robot):
# for hq2 in ht.path:
# st = plan_head_traj(robot, hq2.values)
# new_commands += [st, Scan(robot, o)]
# hq2.step()
# TODO: return to start conf?
elif name == 'localize':
r, _, o, _ = args
new_commands = [Detect(robot, r, o)]
expecting_obs = True
elif name == 'register':
o, p, bq, hq, ht = args
ht0 = plan_head_traj(robot, hq.values)
register = Register(robot, o)
new_commands = [ht0, register]
expecting_obs = True
else:
raise ValueError(name)
saved_world.restore()
for command in new_commands:
if isinstance(command, Trajectory) and command.path:
command.path[-1].step()
commands += new_commands
return commands
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 = create_parser()
parser.add_argument('-problem',
default='rovers1',
help='The name of the problem to solve')
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=120,
type=int,
help='The max time')
parser.add_argument('-enable',
action='store_true',
help='Enables rendering during planning')
parser.add_argument('-teleport',
action='store_true',
help='Teleports between configurations')
parser.add_argument('-simulate',
action='store_true',
help='Simulates the system')
parser.add_argument('-viewer',
action='store_true',
help='Enable the viewer and visualizes the plan')
args = parser.parse_args()
print('Arguments:', 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():
rovers_problem = problem_fn()
saver = WorldSaver()
draw_base_limits(rovers_problem.limits, color=RED)
pddlstream_problem = pddlstream_from_problem(rovers_problem,
collisions=not args.cfree,
teleport=args.teleport,
holonomic=False,
reversible=True,
use_aabb=True)
stream_info = {
'test-cfree-ray-conf': StreamInfo(),
'test-reachable': StreamInfo(p_success=1e-1),
'obj-inv-visible': StreamInfo(),
'com-inv-visible': StreamInfo(),
'sample-above': StreamInfo(),
'sample-motion': StreamInfo(overhead=10),
}
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
#print('Streams:', stream_map.keys())
success_cost = 0 if args.optimal else INF
planner = 'ff-wastar3'
search_sample_ratio = 2
max_planner_time = 10
# TODO: need to accelerate samples here because of the failed test reachable
with Profiler(field='tottime', num=25):
with LockRenderer(lock=not args.enable):
# TODO: option to only consider costs during local optimization
solution = solve(pddlstream_problem,
algorithm=args.algorithm,
stream_info=stream_info,
planner=planner,
max_planner_time=max_planner_time,
debug=False,
unit_costs=args.unit,
success_cost=success_cost,
max_time=args.max_time,
verbose=True,
unit_efforts=True,
effort_weight=1,
search_sample_ratio=search_sample_ratio)
for body in get_bodies():
if body not in saver.bodies:
remove_body(body)
saver.restore()
print_solution(solution)
plan, cost, evaluations = solution
if (plan is None) or not has_gui():
disconnect()
return
# Maybe OpenRAVE didn't actually sample any joints...
# http://openrave.org/docs/0.8.2/openravepy/examples.tutorial_iksolutions/
with LockRenderer():
commands = post_process(rovers_problem, plan)
saver.restore()
wait_for_user('Begin?')
if args.simulate:
control_commands(commands)
else:
time_step = None if args.teleport else 0.01
apply_commands(BeliefState(rovers_problem), commands, time_step)
wait_for_user('Finish?')
disconnect()
def main(display=True, teleport=False):
parser = argparse.ArgumentParser()
parser.add_argument('-problem',
default='rovers1',
help='The name of the problem to solve')
parser.add_argument('-algorithm',
default='focused',
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=120,
type=int,
help='The max time')
parser.add_argument('-unit', action='store_true', help='Uses unit costs')
parser.add_argument('-simulate',
action='store_true',
help='Simulates the system')
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()
saver = WorldSaver()
draw_base_limits(problem.limits, color=(1, 0, 0))
pddlstream_problem = pddlstream_from_problem(problem,
collisions=not args.cfree,
teleport=teleport)
stream_info = {
'test-cfree-ray-conf': StreamInfo(negate=True),
'test-reachable': StreamInfo(p_success=1e-1),
'obj-inv-visible': StreamInfo(),
'com-inv-visible': StreamInfo(),
'sample-above': StreamInfo(),
'sample-motion': StreamInfo(overhead=10),
}
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
#print('Streams:', stream_map.keys())
success_cost = 0 if args.optimal else INF
planner = 'ff-wastar3'
search_sample_ratio = 2
max_planner_time = 10
# TODO: need to accelerate samples here because of the failed test reachable
pr = cProfile.Profile()
pr.enable()
with LockRenderer(False):
if args.algorithm == 'focused':
# TODO: option to only consider costs during local optimization
solution = solve_focused(
pddlstream_problem,
stream_info=stream_info,
planner=planner,
max_planner_time=max_planner_time,
debug=False,
unit_costs=args.unit,
success_cost=success_cost,
max_time=args.max_time,
verbose=True,
unit_efforts=True,
effort_weight=1,
#bind=True, max_skeletons=None,
search_sample_ratio=search_sample_ratio)
elif args.algorithm == 'incremental':
solution = solve_incremental(pddlstream_problem,
planner=planner,
max_planner_time=max_planner_time,
unit_costs=args.unit,
success_cost=success_cost,
max_time=args.max_time,
verbose=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:
return
if (not display) or (plan is None):
disconnect()
return
# Maybe openrave didn't actually sample any joints...
# http://openrave.org/docs/0.8.2/openravepy/examples.tutorial_iksolutions/
with LockRenderer():
commands = post_process(problem, plan, teleport=teleport)
saver.restore() # Assumes bodies are ordered the same way
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
if args.simulate:
control_commands(commands)
else:
time_step = None if teleport else 0.01
apply_commands(BeliefState(problem), commands, time_step)
wait_for_user()
disconnect()
def main(viewer=False, display=True, simulate=False, teleport=False):
# TODO: fix argparse & FastDownward
# parser = argparse.ArgumentParser() # Automatically includes help
# parser.add_argument('-viewer', action='store_true', help='enable viewer.')
# parser.add_argument('-display', action='store_true', help='enable viewer.')
# args = parser.parse_args()
# TODO: getopt
connect(use_gui=viewer)
robot, names, movable = load_world()
saved_world = WorldSaver()
# dump_world()
pddlstream_problem = pddlstream_from_problem(robot,
movable=movable,
teleport=teleport,
movable_collisions=True)
_, _, _, 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:', names)
# initialize and measure performance
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
synthesizers=synthesizers,
max_cost=INF)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
# print stats
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 viewer: # TODO: how to reenable the viewer
disconnect()
connect(use_gui=True)
load_world()
else:
saved_world.restore()
command = postprocess_plan(plan)
#user_input('Execute?')
if simulate:
command.control()
else:
# command.step()
command.refine(num_steps=10).execute(time_step=0.001)
# wait_for_interrupt()
#user_input('Finish?')
disconnect()
def main(viewer=False, display=True, simulate=False, teleport=False):
# TODO: fix argparse & FastDownward
#parser = argparse.ArgumentParser() # Automatically includes help
#parser.add_argument('-viewer', action='store_true', help='enable viewer.')
#parser.add_argument('-display', action='store_true', help='enable viewer.')
#args = parser.parse_args()
connect(use_gui=viewer)
robot, names, movable = load_world()
saved_world = WorldSaver()
dump_world()
pddlstream_problem = pddlstream_from_problem(robot,
movable=movable,
teleport=teleport,
movable_collisions=True)
_, _, _, 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))),
]
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,
max_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):
p.disconnect()
return
paths = []
for name, args in plan:
if name == 'place':
paths += args[-1].reverse().body_paths
elif name in ['move', 'move_free', 'move_holding', 'pick']:
paths += args[-1].body_paths
print(paths)
command = Command(paths)
if not viewer: # TODO: how to reenable the viewer
disconnect()
connect(use_gui=True)
load_world()
saved_world.restore()
user_input('Execute?')
if simulate:
command.control()
else:
#command.step()
command.refine(num_steps=10).execute(time_step=0.001)
#wait_for_interrupt()
user_input('Finish?')
disconnect()
