def display_plan(problem, state, plan, time_step=0.01, sec_per_step=0.005):
duration = compute_duration(plan)
real_time = None if sec_per_step is None else (duration * sec_per_step / time_step)
print('Duration: {} | Step size: {} | Real time: {}'.format(duration, time_step, real_time))
colors = {robot: COLOR_FROM_NAME[robot] for robot in problem.robots}
for i, t in enumerate(inclusive_range(0, duration, time_step)):
print('Step={} | t={}'.format(i, t))
for action in plan:
name, args, start, duration = action
if not (action.start <= t <= get_end(action)):
continue
if name == 'move':
robot, conf1, traj, conf2 = args
traj = [conf1.values, conf2.values]
fraction = (t - action.start) / action.duration
conf = get_value_at_time(traj, fraction)
body = problem.get_body(robot)
color = COLOR_FROM_NAME[robot]
if colors[robot] != color:
set_color(body, color, link_from_name(body, TOP_LINK))
colors[robot] = color
set_base_conf(body, conf)
elif name == 'recharge':
robot, conf = args
body = problem.get_body(robot)
color = YELLOW
if colors[robot] != color:
set_color(body, color, link_from_name(body, TOP_LINK))
colors[robot] = color
else:
raise ValueError(name)
if sec_per_step is None:
user_input('Continue?')
else:
wait_for_duration(sec_per_step)
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:
enable_gravity()
control_commands(commands)
else:
step_commands(commands, time_step=0.01)
user_input('Finish?')
disconnect()
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(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):
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()
else:
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()
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()
