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 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(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 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():
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(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 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('-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 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 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()
planner = 'max-astar'
#solution = solve_incremental(pddlstream_problem, planner=planner, unit_costs=False)
solution = solve_focused(pddlstream_problem,
planner=planner,
unit_costs=False)
print_solution(solution)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
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 test_placement_on_platform(agent):
"""
Tests setting up a PDDL problem with each block on the platform.
Intended to ensure the given domain/stream can complete the experiment
setup.
"""
init = agent._get_initial_pddl_state()
# Create the problem for one block on the platform.
tform = numpy.array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.1],
[0., 0., 0., 1.]])
init += [('RelPose', agent.pddl_blocks[1], agent.platform, tform)]
goal = ('On', agent.pddl_blocks[1], agent.platform)
print('Blocks:', [b.get_name() for b in agent.pddl_blocks])
print('Init:', init)
print('Goal:', goal)
agent.robot.arm.hand.Open()
saved_world = pb_robot.utils.WorldSaver()
pddlstream_problem = tuple([*agent.pddl_info, init, goal])
plan, cost, evaluations = solve_focused(pddlstream_problem,
success_cost=numpy.inf)
if plan is None:
print("No plan found")
else:
saved_world.restore()
input("Execute?")
ExecuteActions(plan)
def plan_trajectories(task, context, collisions=True, max_time=180):
stream_info = {
'TrajPoseCollision': FunctionInfo(p_success=1e-3, eager=False),
'TrajConfCollision': FunctionInfo(p_success=1e-3, eager=False),
}
problem = get_pddlstream_problem(task, context, collisions=collisions)
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(problem,
stream_info=stream_info,
planner='ff-wastar2',
success_cost=INF,
max_time=max_time,
debug=False,
unit_efforts=True,
effort_weight=1,
search_sample_ratio=0)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(5)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
print('Unable to find a solution in under {} seconds'.format(max_time))
return None
return postprocess_plan(task.mbp, task.gripper, plan)
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 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 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 plan_sequence(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
trajectories=[]):
if trajectories is None:
return None
pddlstream_fn = get_pddlstream2 # get_pddlstream | get_pddlstream2
# TODO: Iterated search
# http://www.fast-downward.org/Doc/Heuristic#h.5Em_heuristic
# randomize_successors=True
pr = cProfile.Profile()
pr.enable()
pddlstream_problem = pddlstream_fn(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
trajectories=trajectories)
#solution = solve_exhaustive(pddlstream_problem, planner='goal-lazy', max_time=300, debug=True)
#solution = solve_incremental(pddlstream_problem, planner='add-random-lazy', max_time=600,
# max_planner_time=300, debug=True)
stream_info = {
#'sample-print': StreamInfo(PartialInputs(unique=True)),
}
planner = 'ff-ehc'
#planner = 'ff-wastar1000' # Branching factor becomes large. Rely on preferred. Preferred should also be cheaper
#planner = 'ff-eager-wastar1000'
#planner = 'ff-wastar5'
solution = solve_focused(
pddlstream_problem,
stream_info=stream_info,
max_time=600,
effort_weight=1,
unit_efforts=True,
use_skeleton=False, #unit_costs=True,
planner=planner,
max_planner_time=15,
debug=True)
# solve_exhaustive | solve_incremental
# Reachability heuristics good for detecting dead-ends
# Infeasibility from the start means disconnected or collision
# Random restart based strategy here
print_solution(solution)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
plan, _, _ = solution
if plan is None:
return None
if pddlstream_fn == get_pddlstream:
return [t for _, (n1, e, t, n2) in plan]
elif pddlstream_fn == get_pddlstream2:
return [t for _, (n1, e, t) in reversed(plan)]
else:
raise ValueError(pddlstream_fn)
def main():
# TODO: maybe load problems as a domain explicitly
pddlstream_problem = get_pddlstream_problem()
stream_info = {
#'test-feasible': StreamInfo(negate=True),
}
solution = solve_focused(pddlstream_problem, stream_info=stream_info)
#solution = solve_incremental(pddlstream_problem) # Should throw an error
print_solution(solution)
def plan_commands(state,
viewer=False,
teleport=False,
profile=False,
verbose=True):
# TODO: could make indices into set of bodies to ensure the same...
# TODO: populate the bodies here from state and not the real world
sim_world = connect(use_gui=viewer)
#clone_world(client=sim_world)
task = state.task
robot_conf = get_configuration(task.robot)
robot_pose = get_pose(task.robot)
with ClientSaver(sim_world):
with HideOutput():
robot = create_pr2(use_drake=USE_DRAKE_PR2)
set_pose(robot, robot_pose)
set_configuration(robot, robot_conf)
mapping = clone_world(client=sim_world, exclude=[task.robot])
assert all(i1 == i2 for i1, i2 in mapping.items())
set_client(sim_world)
saved_world = WorldSaver() # StateSaver()
pddlstream_problem = pddlstream_from_state(state, teleport=teleport)
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', sorted(init, key=lambda f: f[0]))
if verbose:
print('Goal:', goal)
print('Streams:', stream_map.keys())
stream_info = {
'test-vis-base': StreamInfo(eager=True, p_success=0),
'test-reg-base': StreamInfo(eager=True, p_success=0),
}
hierarchy = [
ABSTRIPSLayer(pos_pre=['AtBConf']),
]
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
hierarchy=hierarchy,
debug=False,
success_cost=MAX_COST,
verbose=verbose)
plan, cost, evaluations = solution
if MAX_COST <= cost:
plan = None
print_solution(solution)
print('Finite cost:', cost < MAX_COST)
commands = post_process(state, plan)
pr.disable()
if profile:
pstats.Stats(pr).sort_stats('cumtime').print_stats(10)
saved_world.restore()
disconnect()
return commands
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(replan=True, defer=True):
pddlstream_problem = get_pddlstream()
stream_info = {
#'test-pose': StreamInfo(eager=True, p_success=0),
'motion': StreamInfo(defer_fn=defer_unique if defer else never_defer),
}
replan_actions = {'pick'} if replan else set()
with Profiler():
solution = solve_focused(pddlstream_problem, stream_info=stream_info, replan_actions=replan_actions)
print_solution(solution)
def plan_sequence(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
trajectories=[],
collisions=True,
debug=False,
max_time=30):
if trajectories is None:
return None
# TODO: iterated search using random restarts
# TODO: most of the time seems to be spent extracting the stream plan
# TODO: NEGATIVE_SUFFIX to make axioms easier
pr = cProfile.Profile()
pr.enable()
pddlstream_problem = get_pddlstream(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
trajectories=trajectories,
collisions=collisions)
#solution = solve_exhaustive(pddlstream_problem, planner='goal-lazy', max_time=300, debug=True)
#solution = solve_incremental(pddlstream_problem, planner='add-random-lazy', max_time=600,
# max_planner_time=300, debug=True)
stream_info = {
'sample-print': StreamInfo(PartialInputs(unique=True)),
}
#planner = 'ff-ehc'
planner = 'ff-lazy-tiebreak' # Branching factor becomes large. Rely on preferred. Preferred should also be cheaper
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
max_time=max_time,
effort_weight=1,
unit_efforts=True,
use_skeleton=False,
unit_costs=True,
planner=planner,
max_planner_time=15,
debug=debug,
reorder=False)
# Reachability heuristics good for detecting dead-ends
# Infeasibility from the start means disconnected or collision
print_solution(solution)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
plan, _, _ = solution
if plan is None:
return None
return [t for _, (n1, e, t) in reversed(plan)]
def main():
parser = argparse.ArgumentParser()
# choreo_brick_demo | choreo_eth-trees_demo
parser.add_argument('-p',
'--problem',
default='choreo_brick_demo',
help='The name of the problem to solve')
parser.add_argument('-c',
'--cfree',
action='store_true',
help='Disables collisions with obstacles')
parser.add_argument('-t',
'--teleport',
action='store_true',
help='Teleports instead of computing motions')
parser.add_argument('-v',
'--viewer',
action='store_true',
help='Enables the viewer during planning (slow!)')
args = parser.parse_args()
print('Arguments:', args)
connect(use_gui=args.viewer)
robot, brick_from_index, obstacle_from_name = load_pick_and_place(
args.problem)
np.set_printoptions(precision=2)
pr = cProfile.Profile()
pr.enable()
with WorldSaver():
pddlstream_problem = get_pddlstream(robot,
brick_from_index,
obstacle_from_name,
collisions=not args.cfree,
teleport=args.teleport)
with LockRenderer():
solution = solve_focused(pddlstream_problem,
planner='ff-wastar1',
max_time=30)
pr.disable()
pstats.Stats(pr).sort_stats('cumtime').print_stats(10)
print_solution(solution)
plan, _, _ = solution
if plan is None:
return
if not has_gui():
connect(use_gui=True)
_ = load_pick_and_place(args.problem)
step_plan(plan, time_step=(np.inf if args.teleport else 0.01))
def plan_trajectories(task, context, collisions=True, max_time=180):
stream_info = {
'TrajPoseCollision': FunctionInfo(p_success=1e-3),
'TrajConfCollision': FunctionInfo(p_success=1e-3),
}
problem = get_pddlstream_problem(task, context, collisions=collisions)
solution = solve_focused(problem, stream_info=stream_info, planner='ff-wastar2',
max_time=max_time, search_sampling_ratio=0)
print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
print('Unable to find a solution in under {} seconds'.format(max_time))
return None
return postprocess_plan(task.mbp, task.gripper, 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)
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 plan_commands(state, teleport=False, profile=False, verbose=True):
# TODO: could make indices into set of bodies to ensure the same...
# TODO: populate the bodies here from state
task = state.task
robot_conf = get_configuration(task.robot)
robot_pose = get_pose(task.robot)
sim_world = connect(use_gui=False)
with ClientSaver(sim_world):
with HideOutput():
robot = create_pr2(use_drake=USE_DRAKE_PR2)
#robot = load_model(DRAKE_PR2_URDF, fixed_base=True)
set_pose(robot, robot_pose)
set_configuration(robot, robot_conf)
clone_world(client=sim_world, exclude=[task.robot])
set_client(sim_world)
saved_world = WorldSaver() # StateSaver()
pddlstream_problem = pddlstream_from_state(state, teleport=teleport)
_, _, _, stream_map, init, goal = pddlstream_problem
print('Init:', sorted(init, key=lambda f: f[0]))
if verbose:
print('Goal:', goal)
print('Streams:', stream_map.keys())
stream_info = {
'test-vis-base': StreamInfo(eager=True, p_success=0),
'test-reg-base': StreamInfo(eager=True, p_success=0),
}
pr = cProfile.Profile()
pr.enable()
solution = solve_focused(pddlstream_problem,
stream_info=stream_info,
max_cost=MAX_COST,
verbose=verbose)
pr.disable()
plan, cost, evaluations = solution
if MAX_COST <= cost:
plan = None
print_solution(solution)
print('Finite cost:', cost < MAX_COST)
print('Real cost:', float(cost) / SCALE_COST)
if profile:
pstats.Stats(pr).sort_stats('tottime').print_stats(10)
saved_world.restore()
commands = post_process(state, plan)
disconnect()
return commands
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 find_plan(self, ros_goal):
""" Main PDDLStream planning function (DEPRECATED)"""
print("Planning...")
self.plan()
self.robot.arm.hand.Open()
# Get the initial conditions and goal specification
init, goal, fixed_objs = self.unpack_goal(ros_goal)
print_planning_problem(init, goal, fixed_objs)
saved_world = pb_robot.utils.WorldSaver()
# Get PDDLStream planning information
pddl_info = get_pddlstream_info(self.robot,
self.pddl_blocks,
add_slanted_grasps=False,
approach_frame='global')
# Run PDDLStream focused solver
start = time.time()
pddlstream_problem = tuple([*pddl_info, init, goal])
plan, _, _ = solve_focused(pddlstream_problem,
success_cost=numpy.inf,
max_skeletons=2,
max_failures=3,
search_sample_ratio=1.,
max_time=INF)
duration = time.time() - start
saved_world.restore()
print('Planning Complete: Time %f seconds' % duration)
print(f"\nFINAL PLAN\n{plan}\n")
# Package and return the result
result = TaskPlanResult()
result.success = (plan is not None)
result.plan = task_plan_to_ros(plan)
# print(result)
self.planning_service.set_succeeded(result, text="Planning complete")
# Update the planning domain
if result.success:
self.plan()
ExecuteActions(plan,
real=False,
pause=False,
wait=False,
prompt=False)
def main(max_time = 180):
robot = UR5()
robot.open_gripper()
robot.close_gripper()
robot.open_gripper()
robot.go_to_pose_goal_demo()
exit()
problem = get_problem(robot)
solution = solve_focused(problem, planner='ff-wastar2',
success_cost=INF, max_time=max_time, debug=False,
unit_efforts=True, effort_weight=1, search_sample_ratio=0)
# print_solution(solution)
plan, cost, evaluations = solution
if plan is None:
print('Unable to find a solution in under {} seconds'.format(max_time))
return None
interpret_plan(robot, plan)
return True