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)
problem_fn = holding_problem
# holding_problem | stacking_problem | cleaning_problem | cooking_problem
# cleaning_button_problem | cooking_button_problem
problem = problem_fn()
state_id = save_state()
#saved_world = WorldSaver()
dump_world()
pddlstream_problem = pddlstream_from_problem(problem, teleport=teleport)
_, _, _, stream_map, init, goal = pddlstream_problem
synthesizers = [
#StreamSynthesizer('safe-base-motion', {'plan-base-motion': 1,
# 'TrajPoseCollision': 0, 'TrajGraspCollision': 0, 'TrajArmCollision': 0},
# from_fn(get_base_motion_synth(problem, teleport))),
]
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
print('Synthesizers:', synthesizers)
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 viewer:
restore_state(state_id)
else:
disconnect()
connect(use_gui=True)
problem = problem_fn() # TODO: way of doing this without reloading?
user_input('Execute?')
commands = post_process(problem, plan)
if simulate:
control_commands(commands)
else:
apply_commands(State(), commands, time_step=0.01)
user_input('Finish?')
disconnect()
def main(time_step=0.01):
parser = create_parser()
parser.add_argument('-teleport',
action='store_true',
help='Teleports between configurations')
parser.add_argument('-viewer',
action='store_true',
help='enable the viewer while planning')
# TODO: argument for selecting prior
args = parser.parse_args()
print('Arguments:', args)
# TODO: nonuniform distribution to bias towards other actions
# TODO: closed world and open world
real_world = connect(use_gui=not args.viewer)
add_data_path()
task, state = get_problem1(localized='rooms',
p_other=0.25) # surfaces | rooms
for body in task.get_bodies():
add_body_name(body)
robot = task.robot
#dump_body(robot)
assert (USE_DRAKE_PR2 == is_drake_pr2(robot))
attach_viewcone(robot) # Doesn't work for the normal pr2?
draw_base_limits(get_base_limits(robot), color=(0, 1, 0))
#wait_for_user()
# TODO: partially observable values
# TODO: base movements preventing pick without look
# TODO: do everything in local coordinate frame
# TODO: automatically determine an action/command cannot be applied
# TODO: convert numpy arrays into what they are close to
# TODO: compute whether a plan will still achieve a goal and do that
# TODO: update the initial state directly and then just redraw it to ensure uniqueness
step = 0
while True:
step += 1
print('\n' + 50 * '-')
print(step, state)
wait_for_user()
#print({b: p.value for b, p in state.poses.items()})
with ClientSaver():
commands = plan_commands(state, args)
print()
if commands is None:
print('Failure!')
break
if not commands:
print('Success!')
break
apply_commands(state, commands, time_step=time_step)
print(state)
wait_for_user()
disconnect()
def main(time_step=0.01):
# TODO: closed world and open world
real_world = connect(use_gui=True)
add_data_path()
task, state = get_problem1(localized='rooms',
p_other=0.5) # surfaces | rooms
for body in task.get_bodies():
add_body_name(body)
robot = task.robot
#dump_body(robot)
assert (USE_DRAKE_PR2 == is_drake_pr2(robot))
attach_viewcone(robot) # Doesn't work for the normal pr2?
draw_base_limits(get_base_limits(robot), color=(0, 1, 0))
#wait_for_interrupt()
# TODO: partially observable values
# TODO: base movements preventing pick without look
# TODO: do everything in local coordinate frame
# TODO: automatically determine an action/command cannot be applied
# TODO: convert numpy arrays into what they are close to
# TODO: compute whether a plan will still achieve a goal and do that
# TODO: update the initial state directly and then just redraw it to ensure uniqueness
step = 0
while True:
step += 1
print('\n' + 50 * '-')
print(step, state)
#print({b: p.value for b, p in state.poses.items()})
with ClientSaver():
commands = plan_commands(state)
print()
if commands is None:
print('Failure!')
break
if not commands:
print('Success!')
break
apply_commands(state, commands, time_step=time_step)
print(state)
wait_for_interrupt()
disconnect()
def main():
parser = create_parser()
parser.add_argument('-problem', default='problem1', 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():
problem = problem_fn()
saver = WorldSaver()
draw_base_limits(problem.limits, color=RED)
pddlstream_problem = pddlstream_from_problem(problem, collisions=not args.cfree, teleport=args.teleport)
stream_info = {
'inverse-kinematics': StreamInfo(),
'plan-base-motion': StreamInfo(overhead=1e1),
}
_, _, _, 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-astar'
search_sample_ratio = 1
max_planner_time = 10
with Profiler(field='cumtime', num=25): # cumtime | tottime
with LockRenderer(lock=not args.enable):
solution = solve(pddlstream_problem, stream_info=stream_info,
planner=planner, max_planner_time=max_planner_time,
unit_costs=args.unit, success_cost=success_cost,
max_time=args.max_time, verbose=True, debug=False,
unit_efforts=True, effort_weight=1,
search_sample_ratio=search_sample_ratio)
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(lock=not args.enable):
commands = post_process(problem, plan, teleport=args.teleport)
saver.restore()
if args.simulate:
control_commands(commands)
else:
time_step = None if args.teleport else 0.01
apply_commands(BeliefState(problem), commands, time_step)
wait_for_user()
disconnect()
def main():
parser = create_parser(default_algorithm='binding')
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('-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)
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)
stream_info = {
'test-cfree-conf-pose': StreamInfo(p_success=1e-2),
'test-cfree-traj-pose': StreamInfo(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
with Profiler(field='tottime', num=25): # cumtime | tottime
with LockRenderer(lock=not args.enable):
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)
saver.restore()
print_solution(solution)
plan, cost, evaluations = solution
if (plan is None) or not has_gui():
disconnect()
return
with LockRenderer():
commands = post_process(problem, plan)
saver.restore() # Assumes bodies are ordered the same way
wait_for_user()
if args.simulate:
control_commands(commands)
else:
apply_commands(BeliefState(problem), commands,
time_step=1e-2) # 1e-2 | None
wait_for_user()
disconnect()
def main(display=True, teleport=False, partial=False, defer=False):
parser = argparse.ArgumentParser()
parser.add_argument('-simulate',
action='store_true',
help='Simulates the system')
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'),
defer_fn=defer_shared if defer else never_defer),
'MoveCost':
FunctionInfo(opt_move_cost_fn),
} if partial else {
'sample-pose': StreamInfo(from_fn(opt_pose_fn)),
'inverse-kinematics': StreamInfo(from_fn(opt_ik_fn)),
'plan-base-motion': StreamInfo(from_fn(opt_motion_fn)),
'MoveCost': FunctionInfo(opt_move_cost_fn),
}
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
print('Streams:', stream_map.keys())
pr = cProfile.Profile()
pr.enable()
with LockRenderer():
#solution = solve_incremental(pddlstream_problem, debug=True)
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
success_cost=INF,
debug=False)
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
with LockRenderer():
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 args.simulate:
control_commands(commands)
else:
apply_commands(State(), commands, time_step=0.01)
user_input('Finish?')
disconnect()
def main(partial=False, defer=False, verbose=True):
parser = create_parser()
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)
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()
saver = WorldSaver()
#dump_world()
pddlstream_problem = pddlstream_from_problem(problem,
teleport=args.teleport)
stream_info = {
# 'test-cfree-pose-pose': StreamInfo(p_success=1e-3, verbose=verbose),
# 'test-cfree-approach-pose': StreamInfo(p_success=1e-2, verbose=verbose),
# 'test-cfree-traj-pose': StreamInfo(p_success=1e-1, verbose=verbose),
'MoveCost': FunctionInfo(opt_move_cost_fn),
}
stream_info.update({
'sample-pose':
StreamInfo(PartialInputs('?r')),
'inverse-kinematics':
StreamInfo(PartialInputs('?p')),
'plan-base-motion':
StreamInfo(PartialInputs('?q1 ?q2'),
defer_fn=defer_shared if defer else never_defer),
} if partial else {
'sample-pose': StreamInfo(from_fn(opt_pose_fn)),
'inverse-kinematics': StreamInfo(from_fn(opt_ik_fn)),
'plan-base-motion': StreamInfo(from_fn(opt_motion_fn)),
})
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
print('Streams:', str_from_object(set(stream_map)))
print(SEPARATOR)
with Profiler():
with LockRenderer(lock=not args.enable):
solution = solve(pddlstream_problem,
algorithm=args.algorithm,
unit_costs=args.unit,
stream_info=stream_info,
success_cost=INF,
verbose=True,
debug=False)
saver.restore()
print_solution(solution)
plan, cost, evaluations = solution
if (plan is None) or not has_gui():
disconnect()
return
print(SEPARATOR)
with LockRenderer(lock=not args.enable):
commands = post_process(problem, plan)
problem.remove_gripper()
saver.restore()
#restore_state(state_id)
saver.restore()
wait_if_gui('Execute?')
if args.simulate:
control_commands(commands)
else:
apply_commands(State(), commands, time_step=0.01)
wait_if_gui('Finish?')
disconnect()
def main(verbose=True):
# TODO: could work just on postprocessing
# TODO: try the other reachability database
# TODO: option to only consider costs during local optimization
parser = create_parser()
parser.add_argument('-problem',
default='packed',
help='The name of the problem to solve')
parser.add_argument('-n',
'--number',
default=5,
type=int,
help='The number of objects')
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('-teleport',
action='store_true',
help='Teleports between configurations')
parser.add_argument('-enable',
action='store_true',
help='Enables rendering during planning')
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():
problem = problem_fn(num=args.number)
draw_base_limits(problem.base_limits, color=(1, 0, 0))
saver = WorldSaver()
#handles = []
#for link in get_group_joints(problem.robot, 'left_arm'):
# handles.append(draw_link_name(problem.robot, link))
#wait_for_user()
pddlstream_problem = pddlstream_from_problem(problem,
collisions=not args.cfree,
teleport=args.teleport)
stream_info = {
'inverse-kinematics':
StreamInfo(),
'plan-base-motion':
StreamInfo(overhead=1e1),
'test-cfree-pose-pose':
StreamInfo(p_success=1e-3, verbose=verbose),
'test-cfree-approach-pose':
StreamInfo(p_success=1e-2, verbose=verbose),
'test-cfree-traj-pose':
StreamInfo(p_success=1e-1,
verbose=verbose), # TODO: apply to arm and base trajs
#'test-cfree-traj-grasp-pose': StreamInfo(verbose=verbose),
'Distance':
FunctionInfo(p_success=0.99, opt_fn=lambda q1, q2: BASE_CONSTANT),
#'MoveCost': FunctionInfo(lambda t: BASE_CONSTANT),
}
#stream_info = {}
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', init)
print('Goal:', goal)
print('Streams:', str_from_object(set(stream_map)))
success_cost = 0 if args.optimal else INF
planner = 'ff-astar' if args.optimal else 'ff-wastar3'
search_sample_ratio = 2
max_planner_time = 10
# effort_weight = 0 if args.optimal else 1
effort_weight = 1e-3 if args.optimal else 1
with Profiler(field='tottime', num=25): # cumtime | tottime
with LockRenderer(lock=not args.enable):
solution = solve(pddlstream_problem,
algorithm=args.algorithm,
stream_info=stream_info,
planner=planner,
max_planner_time=max_planner_time,
unit_costs=args.unit,
success_cost=success_cost,
max_time=args.max_time,
verbose=True,
debug=False,
unit_efforts=True,
effort_weight=effort_weight,
search_sample_ratio=search_sample_ratio)
saver.restore()
cost_over_time = [(s.cost, s.time) for s in SOLUTIONS]
for i, (cost, runtime) in enumerate(cost_over_time):
print('Plan: {} | Cost: {:.3f} | Time: {:.3f}'.format(
i, cost, runtime))
#print(SOLUTIONS)
print_solution(solution)
plan, cost, evaluations = solution
if (plan is None) or not has_gui():
disconnect()
return
with LockRenderer(lock=not args.enable):
commands = post_process(problem, plan, teleport=args.teleport)
saver.restore()
draw_base_limits(problem.base_limits, color=(1, 0, 0))
wait_for_user()
if args.simulate:
control_commands(commands)
else:
time_step = None if args.teleport else 0.01
apply_commands(State(), commands, time_step)
wait_for_user()
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(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 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()
