def collect_scoop(args, ros_world):
arm, _ = ACTIVE_ARMS['scoop']
cup_name, bowl_name = get_scoop_items(ros_world)
spoon_name = create_name(args.spoon,
1) # grey_spoon | orange_spoon | green_spoon
init_holding = get_spoon_init_holding(arm, spoon_name)
init = [
('Contains', bowl_name, SUGAR),
]
goal = [
('Contains', cup_name, SUGAR),
]
skeleton = [
('move-arm', [arm, X, X, X]),
('scoop', [arm, bowl_name, X, spoon_name, X, SUGAR, X, X, X]),
('move-arm', [arm, X, X, X]),
('pour', [arm, cup_name, X, spoon_name, X, SUGAR, X, X, X]),
('move-arm', [arm, X, X, X]),
]
constraints = PlanConstraints(skeletons=[skeleton], exact=True)
task = Task(init=init,
init_holding=init_holding,
goal=goal,
arms=[arm],
required=[cup_name, bowl_name],
reset_arms=True,
use_scales=True,
constraints=constraints)
add_holding(task, ros_world)
feature = get_scoop_feature(ros_world, bowl_name, spoon_name)
return task, feature
def solve_current(problem,
constraints=PlanConstraints(),
unit_costs=False,
verbose=False,
**search_args):
"""
Solves a PDDLStream problem without applying any streams
Will fail if the problem requires stream applications
:param problem: a PDDLStream problem
:param constraints: PlanConstraints on the available solutions
:param unit_costs: use unit action costs rather than numeric costs
:param verbose: if True, this prints the result of each stream application
:param search_args: keyword args for the search subroutine
:return: a tuple (plan, cost, evaluations) where plan is a sequence of actions
(or None), cost is the cost of the plan, and evaluations is init but expanded
using stream applications
"""
evaluations, goal_expression, domain, externals = parse_problem(
problem, constraints=constraints, unit_costs=unit_costs)
instantiator = Instantiator(externals, evaluations)
process_function_queue(instantiator, evaluations, verbose=verbose)
plan, cost = solve_finite(evaluations,
goal_expression,
domain,
max_cost=constraints.max_cost,
**search_args)
return revert_solution(plan, cost, evaluations)
def examine_instantiated(problem, constraints=PlanConstraints(), unit_costs=False, unique=False, verbose=False, debug=False):
# TODO: refactor to an analysis file
domain_pddl, constant_map, stream_pddl, _, init, goal = problem
stream_map = DEBUG if unique else SHARED_DEBUG # DEBUG_MODES
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
evaluations, goal_exp, domain, externals = parse_problem(
problem, constraints=constraints, unit_costs=unit_costs)
assert not isinstance(domain, SimplifiedDomain)
# store = SolutionStore(evaluations, max_time, success_cost=INF, verbose=verbose)
# instantiator = Instantiator(externals, evaluations)
# process_stream_queue(instantiator, store, complexity_limit=INF, verbose=verbose)
# results = [] # TODO: extract from process_stream_queue
#set_unique(externals)
# domain.actions[:] = [] # TODO: only instantiate axioms
# TODO: drop all fluents and instantiate
# TODO: relaxed planning version of this
results, exhausted = optimistic_process_streams(evaluations, externals, complexity_limit=INF, max_effort=None)
evaluations = evaluations_from_stream_plan(evaluations, results, max_effort=None)
problem = get_problem(evaluations, goal_exp, domain, unit_costs)
task = task_from_domain_problem(domain, problem)
with Verbose(debug):
instantiated = instantiate_task(task)
if instantiated is None:
return None
# TODO: reinstantiate actions?
instantiated.axioms[:] = [reinstantiate_axiom(axiom) for axiom in instantiated.axioms]
instantiated = convert_instantiated(instantiated)
return results, instantiated
def collect_pour(args, ros_world):
arm, _ = ACTIVE_ARMS['pour']
cup_name, bowl_name = get_pour_items(ros_world)
init = [
('Contains', cup_name, COFFEE),
]
goal = [
('Contains', bowl_name, COFFEE),
]
skeleton = [
('move-arm', [arm, X, X, X]),
('pick', [arm, cup_name, X, X, X, X, X]),
('move-arm', [arm, X, X, X]),
('pour', [arm, bowl_name, X, cup_name, X, COFFEE, X, X, X]),
('move-arm', [arm, X, X, X]),
('place', [arm, cup_name, X, X, X, X, X, X, X, X]),
('move-arm', [arm, X, X, X]),
]
constraints = PlanConstraints(skeletons=[skeleton], exact=True)
# TODO: remove required
# TODO: ensure the robot reaches its start configuration
task = Task(
init=init,
goal=goal,
arms=[arm],
required=[cup_name, bowl_name], # TODO: table?
reset_arms=True,
empty_arms=True,
use_scales=True,
constraints=constraints)
feature = get_pour_feature(ros_world, bowl_name, cup_name)
return task, feature
def compute_skeleton_constraints(action_plan, bindings):
skeleton = []
groups = {
arg: values
for arg, values in bindings.items() if len(values) != 1
}
for name, args in action_plan:
new_args = []
for arg in args:
if isinstance(arg, Object):
new_args.append(arg)
elif isinstance(arg, OptimisticObject):
assert bindings.get(arg, [])
if len(bindings[arg]) == 1:
new_args.append(bindings[arg][0])
else:
#new_args.append(WILD)
new_args.append(arg)
else:
raise ValueError(arg)
skeleton.append((name, new_args))
# exact=False because we might need new actions
return PlanConstraints(skeletons=[skeleton],
groups=groups,
exact=False,
max_cost=INF)
def compute_skeleton_constraints(opt_plan, bindings):
skeleton = []
groups = {
arg: values
for arg, values in bindings.items() if len(values) != 1
}
action_plan, preimage_facts = opt_plan
for name, args in action_plan:
new_args = []
for arg in args:
if isinstance(arg, Object):
new_args.append(arg)
elif isinstance(arg, OptimisticObject):
new_args.append(WILD)
# TODO: might cause some strange effects on continuous_tamp -p blocked
#assert bindings.get(arg, [])
#if len(bindings[arg]) == 1:
# new_args.append(bindings[arg][0])
#else:
# #new_args.append(WILD)
# new_args.append(arg)
else:
raise ValueError(arg)
skeleton.append((name, new_args))
# exact=False because we might need new actions
return PlanConstraints(skeletons=[skeleton],
groups=groups,
exact=False,
max_cost=INF)
def examine_instantiated(problem,
constraints=PlanConstraints(),
unit_costs=False,
max_time=INF,
verbose=False,
**search_args):
domain_pddl, constant_map, stream_pddl, _, init, goal = problem
stream_map = DEBUG
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
evaluations, goal_exp, domain, externals = parse_problem(
problem, constraints=constraints, unit_costs=unit_costs)
store = SolutionStore(evaluations,
max_time,
success_cost=INF,
verbose=verbose)
#externals = compile_fluents_as_attachments(domain, externals) #
instantiator = Instantiator(externals, evaluations)
process_stream_queue(instantiator,
store,
complexity_limit=INF,
verbose=verbose)
#plan, cost = solve_finite(evaluations, goal_exp, domain, max_cost=max_cost, **search_args)
debug = False
assert not isinstance(domain, SimplifiedDomain)
problem = get_problem(evaluations, goal_exp, domain, unit_costs)
task = task_from_domain_problem(domain, problem)
with Verbose(debug):
instantiated = instantiate_task(task)
instantiated = convert_instantiated(instantiated)
return instantiated
def collect_push(args, ros_world):
arm = LEFT_ARM
block_name, = get_pour_items(ros_world)
goal_pos2d = np.random.uniform(*POSE2D_RANGE)[:2]
init = [('CanPush', block_name, goal_pos2d)]
goal = [('InRegion', block_name, goal_pos2d)]
skeleton = [
('move-arm', [arm, X, X, X]),
('push', [arm, block_name, X, X, X, X, X]),
('move-arm', [arm, X, X, X]),
]
constraints = PlanConstraints(skeletons=[skeleton], exact=True)
task = Task(init=init,
goal=goal,
arms=[arm],
required=[block_name],
reset_arms=True,
use_scales=False,
constraints=constraints)
initial_pose = ros_world.perception.get_pose(block_name)
feature = get_push_feature(ros_world, block_name, arm, initial_pose,
goal_pos2d)
for goal in task.goal:
if goal[0] == 'InRegion':
_, block_name, goal_pos2d = goal
draw_push_goal(ros_world, block_name, goal_pos2d)
return task, feature
def solve_incremental(problem, constraints=PlanConstraints(),
unit_costs=False, success_cost=INF,
max_iterations=INF, max_time=INF,
start_complexity=0, complexity_step=1, max_complexity=INF,
verbose=False, **search_args):
"""
Solves a PDDLStream problem by alternating between applying all possible streams and searching
:param problem: a PDDLStream problem
:param constraints: PlanConstraints on the set of legal solutions
:param max_time: the maximum amount of time to apply streams
:param max_iterations: the maximum amount of search iterations
:param unit_costs: use unit action costs rather than numeric costs
:param success_cost: an exclusive (strict) upper bound on plan cost to terminate
:param start_complexity: the stream complexity on the first iteration
:param complexity_step: the increase in the complexity limit after each iteration
:param max_complexity: the maximum stream complexity
:param verbose: if True, this prints the result of each stream application
:param search_args: keyword args for the search subroutine
:return: a tuple (plan, cost, evaluations) where plan is a sequence of actions
(or None), cost is the cost of the plan, and evaluations is init but expanded
using stream applications
"""
# max_complexity = 0 => current
# complexity_step = INF => exhaustive
# success_cost = terminate_cost = decision_cost
evaluations, goal_expression, domain, externals = parse_problem(
problem, constraints=constraints, unit_costs=unit_costs)
store = SolutionStore(evaluations, max_time, success_cost, verbose) # TODO: include other info here?
ensure_no_fluent_streams(externals)
if UPDATE_STATISTICS:
load_stream_statistics(externals)
num_iterations = num_calls = 0
complexity_limit = start_complexity
instantiator = Instantiator(externals, evaluations)
num_calls += process_stream_queue(instantiator, store, complexity_limit, verbose=verbose)
while not store.is_terminated() and (num_iterations <= max_iterations) and (complexity_limit <= max_complexity):
num_iterations += 1
print('Iteration: {} | Complexity: {} | Calls: {} | Evaluations: {} | Solved: {} | Cost: {} | Time: {:.3f}'.format(
num_iterations, complexity_limit, num_calls, len(evaluations),
store.has_solution(), store.best_cost, store.elapsed_time()))
plan, cost = solve_finite(evaluations, goal_expression, domain,
max_cost=min(store.best_cost, constraints.max_cost), **search_args)
if is_plan(plan):
store.add_plan(plan, cost)
if not instantiator:
break
if complexity_step is None:
# TODO: option to select the next k-smallest complexities
complexity_limit = instantiator.min_complexity()
else:
complexity_limit += complexity_step
num_calls += process_stream_queue(instantiator, store, complexity_limit, verbose=verbose)
#retrace_stream_plan(store, domain, goal_expression)
#print('Final queue size: {}'.format(len(instantiator)))
if UPDATE_STATISTICS:
write_stream_statistics(externals, verbose)
return store.extract_solution()
def main(max_time=20):
"""
Creates and solves the 2D motion planning problem.
"""
parser = create_parser()
args = parser.parse_args()
print('Arguments:', args)
obstacles = [create_box((.5, .5), (.2, .2))]
regions = {
'env': create_box((.5, .5), (1, 1)),
'green': create_box((.8, .8), (.4, .4)),
}
goal = 'green'
if goal not in regions:
goal = ARRAY([1, 1])
max_distance = 0.25 # 0.2 | 0.25 | 0.5 | 1.0
problem, samples, roadmap = create_problem(goal,
obstacles,
regions,
max_distance=max_distance)
print('Initial:', str_from_object(problem.init))
print('Goal:', str_from_object(problem.goal))
constraints = PlanConstraints(max_cost=1.25) # max_cost=INF)
with Profiler(field='tottime', num=10):
solution = solve_incremental(problem,
constraints=constraints,
unit_costs=args.unit,
success_cost=0,
max_time=max_time,
verbose=False)
print_solution(solution)
plan, cost, evaluations = solution
#viewer = draw_environment(obstacles, regions)
#for sample in samples:
# viewer.draw_point(sample)
#user_input('Continue?')
# TODO: use the same viewer here
draw_roadmap(roadmap, obstacles, regions) # TODO: do this in realtime
user_input('Continue?')
if plan is None:
return
segments = [args for name, args in plan]
draw_solution(segments, obstacles, regions)
user_input('Finish?')
def __init__(self,
init=[],
goal=[],
init_holding={},
arms=[LEFT_ARM],
required=[],
stackable=[],
graspable=[],
pushable=[],
stirrable=[],
can_scoop=[],
scoopable=[],
pressable=[],
pourable=[],
can_contain=[],
use_kitchen=False,
use_scales=False,
reset_arms=False,
empty_arms=False,
reset_items=False,
constraints=PlanConstraints()):
self.init = init
self.goal = goal
self.arms = arms
self.init_holding = init_holding
self.required = set(required)
self.stackable = [TABLE] + list(stackable)
self.graspable = ['greenblock', 'cup'] + list(
graspable) # 'spoon', 'stirrer'
self.pushable = ['purpleblock'] + list(pushable)
self.stirrable = ['spoon', 'stirrer'] + list(stirrable)
self.can_scoop = ['spoon'] + list(can_scoop)
self.scoopable = ['bowl'] + list(scoopable)
self.pressable = ['button'] + list(pressable)
self.can_contain = ['bowl'] + list(can_contain) # 'cup'
self.pourable = ['cup', 'spoon'] + list(pourable) # 'bowl'
self.use_kitchen = use_kitchen
self.use_scales = use_scales
self.reset_arms = reset_arms
self.empty_arms = select_values(empty_arms, self.arms)
self.reset_items = select_values(reset_items, self.graspable)
self.constraints = constraints
def solve_exhaustive(problem,
constraints=PlanConstraints(),
unit_costs=False,
max_time=300,
verbose=False,
**search_args):
"""
Solves a PDDLStream problem by applying all possible streams and searching once
Requires a finite max_time when infinitely many stream instances
:param problem: a PDDLStream problem
:param constraints: PlanConstraints on the available solutions
:param unit_costs: use unit action costs rather than numeric costs
:param max_time: the maximum amount of time to apply streams
:param verbose: if True, this prints the result of each stream application
:param search_args: keyword args for the search subroutine
:return: a tuple (plan, cost, evaluations) where plan is a sequence of actions
(or None), cost is the cost of the plan, and evaluations is init but expanded
using stream applications
"""
start_time = time.time()
evaluations, goal_expression, domain, externals = parse_problem(
problem, constraints=constraints, unit_costs=unit_costs)
ensure_no_fluent_streams(externals)
if UPDATE_STATISTICS:
load_stream_statistics(externals)
instantiator = Instantiator(externals, evaluations)
while instantiator.stream_queue and (elapsed_time(start_time) < max_time):
process_instance(instantiator,
evaluations,
instantiator.pop_stream(),
verbose=verbose)
process_function_queue(instantiator, evaluations, verbose=verbose)
plan, cost = solve_finite(evaluations,
goal_expression,
domain,
max_cost=constraints.max_cost,
**search_args)
if UPDATE_STATISTICS:
write_stream_statistics(externals, verbose)
return revert_solution(plan, cost, evaluations)
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-a',
'--algorithm',
default='focused',
help='Specifies the algorithm')
parser.add_argument('-g',
'--gurobi',
action='store_true',
help='Uses gurobi')
parser.add_argument('-o',
'--optimal',
action='store_true',
help='Runs in an anytime mode')
parser.add_argument('-s',
'--skeleton',
action='store_true',
help='Enforces skeleton plan constraints')
# TODO: test if placed in the same region
defer_fn = defer_shared # never_defer | defer_unique | defer_shared
tamp_problem, args = initialize(parser)
stream_info = {
's-region':
StreamInfo(defer_fn=defer_fn),
's-grasp':
StreamInfo(defer_fn=defer_fn),
's-ik':
StreamInfo(defer_fn=get_defer_all_unbound(
inputs='?g')), # defer_fn | defer_unbound
's-motion':
StreamInfo(defer_fn=get_defer_any_unbound()),
't-cfree':
StreamInfo(defer_fn=get_defer_any_unbound(), eager=False,
negate=True), # defer_fn | defer_unbound
't-region':
StreamInfo(eager=False, p_success=0), # bound_fn is None
'dist':
FunctionInfo(defer_fn=get_defer_any_unbound(),
opt_fn=lambda q1, q2: MOVE_COST),
'gurobi-cfree':
StreamInfo(eager=False, negate=True),
#'gurobi': OptimizerInfo(p_success=0),
#'rrt': OptimizerInfo(p_success=0),
}
hierarchy = [
#ABSTRIPSLayer(pos_pre=['atconf']), #, horizon=1),
]
skeletons = [TIGHT_SKELETON] if args.skeleton else None
assert implies(args.skeleton, args.problem == 'tight')
max_cost = INF # 8*MOVE_COST
constraints = PlanConstraints(
skeletons=skeletons,
#skeletons=[],
#skeletons=[skeleton, []],
exact=True,
max_cost=max_cost)
#replan_actions = set()
replan_actions = {'move', 'pick', 'place'}
pddlstream_problem = pddlstream_from_tamp(tamp_problem,
collisions=not args.cfree,
use_stream=not args.gurobi,
use_optimizer=args.gurobi)
dump_pddlstream(pddlstream_problem)
pr = cProfile.Profile()
pr.enable()
success_cost = 0 if args.optimal else INF
planner = 'max-astar'
#planner = 'ff-wastar1'
if args.algorithm == 'focused':
solver = solve_focused # solve_focused | solve_serialized
solution = solver(
pddlstream_problem,
constraints=constraints,
stream_info=stream_info,
replan_actions=replan_actions,
planner=planner,
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=True,
effort_weight=1,
search_sample_ratio=1,
#max_skeletons=None, bind=True,
visualize=args.visualize)
elif args.algorithm == 'incremental':
solution = solve_incremental(pddlstream_problem,
constraints=constraints,
complexity_step=2,
planner=planner,
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('cumtime').print_stats(20)
if plan is not None:
display_plan(tamp_problem, retime_plan(plan))
def solve_pddlstream(belief,
problem,
args,
skeleton=None,
replan_actions=set(),
max_time=INF,
max_memory=MAX_MEMORY,
max_cost=INF):
set_cost_scale(COST_SCALE)
reset_globals()
stream_info = get_stream_info()
#print(set(stream_map) - set(stream_info))
skeletons = create_ordered_skeleton(skeleton)
max_cost = min(max_cost, COST_BOUND)
print('Max cost: {:.3f} | Max runtime: {:.3f}'.format(max_cost, max_time))
constraints = PlanConstraints(skeletons=skeletons,
max_cost=max_cost,
exact=True)
success_cost = 0 if args.anytime else INF
planner = 'ff-astar' if args.anytime else 'ff-wastar2'
search_sample_ratio = 0.5 # 0.5
max_planner_time = 10
# TODO: max number of samples per iteration flag
# TODO: don't greedily expand samples with too high of a complexity if out of time
pr = cProfile.Profile()
pr.enable()
saver = WorldSaver()
sim_state = belief.sample_state()
sim_state.assign()
wait_for_duration(0.1)
with LockRenderer(lock=not args.visualize):
# TODO: option to only consider costs during local optimization
# effort_weight = 0 if args.anytime else 1
effort_weight = 1e-3 if args.anytime else 1
#effort_weight = 0
#effort_weight = None
solution = solve_focused(
problem,
constraints=constraints,
stream_info=stream_info,
replan_actions=replan_actions,
initial_complexity=5,
planner=planner,
max_planner_time=max_planner_time,
unit_costs=args.unit,
success_cost=success_cost,
max_time=max_time,
max_memory=max_memory,
verbose=True,
debug=False,
unit_efforts=True,
effort_weight=effort_weight,
max_effort=INF,
# bind=True, max_skeletons=None,
search_sample_ratio=search_sample_ratio)
saver.restore()
# print([(s.cost, s.time) for s in SOLUTIONS])
# print(SOLUTIONS)
print_solution(solution)
pr.disable()
pstats.Stats(pr).sort_stats('tottime').print_stats(25) # cumtime | tottime
return solution
def main():
# TODO: side grasps (horizontal gripper, one finger, forklift)
parser = create_parser()
parser.add_argument('-g',
'--gurobi',
action='store_true',
help='Uses gurobi')
parser.add_argument('-o',
'--optimal',
action='store_true',
help='Runs in an anytime mode')
parser.add_argument('-s',
'--skeleton',
action='store_true',
help='Enforces skeleton plan constraints')
tamp_problem, args = initialize(parser)
# TODO: test if placed in the same region
defer_fn = defer_shared # never_defer | defer_unique | defer_shared
stream_info = {
's-region':
StreamInfo(defer_fn=defer_fn),
's-grasp':
StreamInfo(defer_fn=defer_fn),
's-ik':
StreamInfo(defer_fn=get_defer_all_unbound(
inputs='?g')), # defer_fn | defer_unbound
's-motion':
StreamInfo(defer_fn=get_defer_any_unbound()),
't-cfree':
StreamInfo(defer_fn=get_defer_any_unbound(),
eager=False,
verbose=False), # defer_fn | defer_unbound
't-region':
StreamInfo(eager=True, p_success=0), # bound_fn is None
'dist':
FunctionInfo(eager=False,
defer_fn=get_defer_any_unbound(),
opt_fn=lambda q1, q2: MOVE_COST),
'gurobi-cfree':
StreamInfo(
eager=False, negate=True
), # TODO: AttributeError: 'tuple' object has no attribute 'instance'
#'gurobi': OptimizerInfo(p_success=0),
#'rrt': OptimizerInfo(p_success=0),
}
#stream_info = {}
hierarchy = [
#ABSTRIPSLayer(pos_pre=['atconf']), #, horizon=1),
]
skeletons = [TIGHT_SKELETON] if args.skeleton else None
assert implies(args.skeleton, args.problem == 'tight')
max_cost = INF # 8*MOVE_COST
constraints = PlanConstraints(
skeletons=skeletons,
#skeletons=[],
#skeletons=[skeleton, []],
exact=True,
max_cost=max_cost)
replan_actions = set()
#replan_actions = {'move', 'pick', 'place'}
pddlstream_problem = pddlstream_from_tamp(tamp_problem,
collisions=not args.cfree,
use_stream=not args.gurobi,
use_optimizer=args.gurobi)
dump_pddlstream(pddlstream_problem)
success_cost = 0 if args.optimal else INF
#planner = 'dijkstra'
planner = 'max-astar'
#planner = 'ff-wastar1'
#effort_weight = 1.
effort_weight = 1. / get_cost_scale()
#effort_weight = None
with Profiler(field='cumtime', num=20):
solution = solve(pddlstream_problem,
algorithm=args.algorithm,
constraints=constraints,
stream_info=stream_info,
replan_actions=replan_actions,
planner=planner,
max_planner_time=10,
hierarchy=hierarchy,
max_time=args.max_time,
max_iterations=INF,
debug=False,
verbose=True,
unit_costs=args.unit,
success_cost=success_cost,
unit_efforts=True,
effort_weight=effort_weight,
search_sample_ratio=1,
visualize=args.visualize) # TODO: solve_serialized
print_solution(solution)
plan, cost, evaluations = solution
if plan is not None:
display_plan(tamp_problem, retime_plan(plan))
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')
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('-g',
'--gurobi',
action='store_true',
help='Uses gurobi')
parser.add_argument('-n',
'--number',
default=2,
type=int,
help='The number of blocks')
parser.add_argument('-o',
'--optimal',
action='store_true',
help='Runs in an anytime mode')
parser.add_argument('-s',
'--skeleton',
action='store_true',
help='Enforces skeleton plan constraints')
parser.add_argument('-t',
'--max_time',
default=30,
type=int,
help='The max time')
parser.add_argument('-u',
'--unit',
action='store_true',
help='Uses unit costs')
parser.add_argument('-v',
'--visualize',
action='store_true',
help='Visualizes graphs')
args = parser.parse_args()
print('Arguments:', args)
np.set_printoptions(precision=2)
if args.deterministic:
random.seed(seed=0)
np.random.seed(seed=0)
print('Random seed:', get_random_seed())
problem_from_name = {fn.__name__: fn for fn in PROBLEMS}
if args.problem not in problem_from_name:
raise ValueError(args.problem)
print('Problem:', args.problem)
problem_fn = problem_from_name[args.problem]
tamp_problem = problem_fn(args.number)
print(tamp_problem)
action_info = {
#'move': ActionInfo(terminal=True),
#'pick': ActionInfo(terminal=True),
#'place': ActionInfo(terminal=True),
}
stream_info = {
't-region': StreamInfo(eager=False, p_success=0), # bound_fn is None
't-cfree': StreamInfo(eager=False, negate=True),
'distance': FunctionInfo(opt_fn=lambda q1, q2: MOVE_COST),
'gurobi-cfree': StreamInfo(eager=False, negate=True),
#'gurobi': OptimizerInfo(p_success=0),
#'rrt': OptimizerInfo(p_success=0),
}
hierarchy = [
#ABSTRIPSLayer(pos_pre=['atconf']), #, horizon=1),
]
skeletons = [TIGHT_SKELETON] if args.skeleton else None
max_cost = INF # 8*MOVE_COST
constraints = PlanConstraints(
skeletons=skeletons,
#skeletons=[],
#skeletons=[skeleton, []],
exact=True,
max_cost=max_cost)
pddlstream_problem = pddlstream_from_tamp(tamp_problem,
collisions=not args.cfree,
use_stream=not args.gurobi,
use_optimizer=args.gurobi)
print('Initial:', sorted_str_from_list(pddlstream_problem.init))
print('Goal:', str_from_object(pddlstream_problem.goal))
pr = cProfile.Profile()
pr.enable()
success_cost = 0 if args.optimal else INF
planner = 'max-astar'
#planner = 'ff-wastar1'
if args.algorithm == 'focused':
solution = solve_focused(
pddlstream_problem,
constraints=constraints,
action_info=action_info,
stream_info=stream_info,
planner=planner,
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=1,
#max_skeletons=None, bind=True,
visualize=args.visualize)
elif args.algorithm == 'incremental':
solution = solve_incremental(pddlstream_problem,
constraints=constraints,
complexity_step=2,
planner=planner,
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('cumtime').print_stats(20)
if plan is not None:
display_plan(tamp_problem, retime_plan(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)
def solve(problem, algorithm=DEFAULT_ALGORITHM, constraints=PlanConstraints(),
stream_info={}, replan_actions=set(),
unit_costs=False, success_cost=INF,
max_time=INF, max_iterations=INF, max_memory=INF,
initial_complexity=0, complexity_step=1, max_complexity=INF,
max_skeletons=INF, search_sample_ratio=1, max_failures=0,
unit_efforts=False, max_effort=INF, effort_weight=None, reorder=True,
#temp_dir=TEMP_DIR, clean=False, debug=False, hierarchy=[],
#planner=DEFAULT_PLANNER, max_planner_time=DEFAULT_MAX_TIME, max_cost=INF, debug=False
visualize=False, verbose=True, **search_kwargs):
"""
Solves a PDDLStream problem generically using one of the available algorithms
:param problem: a PDDLStream problem
:param algorithm: a PDDLStream algorithm name
:param constraints: PlanConstraints on the set of legal solutions
:param stream_info: a dictionary from stream name to StreamInfo altering how individual streams are handled
:param replan_actions: the actions declared to induce replanning for the purpose of deferred stream evaluation
:param unit_costs: use unit action costs rather than numeric costs
:param success_cost: the exclusive (strict) upper bound on plan cost to successfully terminate
:param max_time: the maximum runtime
:param max_iterations: the maximum number of search iterations
:param max_memory: the maximum amount of memory
:param initial_complexity: the initial stream complexity limit
:param complexity_step: the increase in the stream complexity limit per iteration
:param max_complexity: the maximum stream complexity limit
:param max_skeletons: the maximum number of plan skeletons (max_skeletons=None indicates not adaptive)
:param search_sample_ratio: the desired ratio of sample time / search time when max_skeletons!=None
:param max_failures: the maximum number of stream failures before switching phases when max_skeletons=None
:param unit_efforts: use unit stream efforts rather than estimated numeric efforts
:param max_effort: the maximum amount of stream effort
:param effort_weight: a multiplier for stream effort compared to action costs
:param reorder: if True, reorder stream plans to minimize the expected sampling overhead
:param visualize: if True, draw the constraint network and stream plan as a graphviz file
:param verbose: if True, print the result of each stream application
:param search_kwargs: keyword args for the search subroutine
:return: a tuple (plan, cost, evaluations) where plan is a sequence of actions
(or None), cost is the cost of the plan (INF if no plan), and evaluations is init expanded
using stream applications
"""
# TODO: print the arguments using locals()
# TODO: could instead make common arguments kwargs but then they could have different default values
# TODO: portfolios of PDDLStream algorithms
if algorithm == 'incremental':
return solve_incremental(
problem=problem, constraints=constraints,
unit_costs=unit_costs, success_cost=success_cost,
max_iterations=max_iterations, max_time=max_time, max_memory=max_memory,
initial_complexity=initial_complexity, complexity_step=complexity_step, max_complexity=max_complexity,
verbose=verbose, **search_kwargs)
# if algorithm == 'abstract_focused': # meta_focused | meta_focused
# return solve_focused(
# problem, constraints=constraints,
# stream_info=stream_info, replan_actions=replan_actions,
# unit_costs=unit_costs, success_cost=success_cost,
# max_time=max_time, max_iterations=max_iterations, max_memory=max_memory,
# initial_complexity=initial_complexity, complexity_step=complexity_step, #max_complexity=max_complexity,
# max_skeletons=max_skeletons, search_sample_ratio=search_sample_ratio,
# bind=bind, max_failures=max_failures,
# unit_efforts=unit_efforts, max_effort=max_effort, effort_weight=effort_weight, reorder=reorder,
# visualize=visualize, verbose=verbose, **search_kwargs)
fail_fast = (max_failures < INF)
if algorithm == 'focused':
return solve_focused_original(
problem, constraints=constraints,
stream_info=stream_info, replan_actions=replan_actions,
unit_costs=unit_costs, success_cost=success_cost,
max_time=max_time, max_iterations=max_iterations, max_memory=max_memory,
initial_complexity=initial_complexity, complexity_step=complexity_step, max_complexity=max_complexity,
#max_skeletons=max_skeletons, search_sample_ratio=search_sample_ratio,
fail_fast=fail_fast, # bind=bind, max_failures=max_failures,
unit_efforts=unit_efforts, max_effort=max_effort, effort_weight=effort_weight, reorder=reorder,
visualize=visualize, verbose=verbose, **search_kwargs)
if algorithm == 'binding':
return solve_binding(
problem, constraints=constraints,
stream_info=stream_info, replan_actions=replan_actions,
unit_costs=unit_costs, success_cost=success_cost,
max_time=max_time, max_iterations=max_iterations, max_memory=max_memory,
initial_complexity=initial_complexity, complexity_step=complexity_step, max_complexity=max_complexity,
#max_skeletons=max_skeletons, search_sample_ratio=search_sample_ratio,
fail_fast=fail_fast, # bind=bind, max_failures=max_failures,
unit_efforts=unit_efforts, max_effort=max_effort, effort_weight=effort_weight, reorder=reorder,
visualize=visualize, verbose=verbose, **search_kwargs)
if algorithm == 'adaptive':
return solve_adaptive(
problem, constraints=constraints,
stream_info=stream_info, replan_actions=replan_actions,
unit_costs=unit_costs, success_cost=success_cost,
max_time=max_time, max_iterations=max_iterations, max_memory=max_memory,
initial_complexity=initial_complexity, complexity_step=complexity_step, max_complexity=max_complexity,
max_skeletons=max_skeletons, search_sample_ratio=search_sample_ratio,
#bind=bind, max_failures=max_failures,
unit_efforts=unit_efforts, max_effort=max_effort, effort_weight=effort_weight, reorder=reorder,
visualize=visualize, verbose=verbose, **search_kwargs)
raise NotImplementedError(algorithm)
def solve_incremental(problem,
constraints=PlanConstraints(),
unit_costs=False,
success_cost=INF,
max_iterations=INF,
max_time=INF,
max_memory=INF,
initial_complexity=0,
complexity_step=1,
max_complexity=INF,
verbose=False,
**search_kwargs):
"""
Solves a PDDLStream problem by alternating between applying all possible streams and searching
:param problem: a PDDLStream problem
:param constraints: PlanConstraints on the set of legal solutions
:param unit_costs: use unit action costs rather than numeric costs
:param success_cost: the exclusive (strict) upper bound on plan cost to successfully terminate
:param max_time: the maximum runtime
:param max_iterations: the maximum number of search iterations
:param max_memory: the maximum amount of memory
:param initial_complexity: the initial stream complexity limit
:param complexity_step: the increase in the stream complexity limit per iteration
:param max_complexity: the maximum stream complexity limit
:param verbose: if True, print the result of each stream application
:param search_kwargs: keyword args for the search subroutine
:return: a tuple (plan, cost, evaluations) where plan is a sequence of actions
(or None), cost is the cost of the plan (INF if no plan), and evaluations is init expanded
using stream applications
"""
# max_complexity = 0 => current
# complexity_step = INF => exhaustive
# success_cost = terminate_cost = decision_cost
# TODO: warning if optimizers are present
evaluations, goal_expression, domain, externals = parse_problem(
problem, constraints=constraints, unit_costs=unit_costs)
store = SolutionStore(
evaluations, max_time, success_cost, verbose,
max_memory=max_memory) # TODO: include other info here?
if UPDATE_STATISTICS:
load_stream_statistics(externals)
static_externals = compile_fluents_as_attachments(domain, externals)
num_iterations = num_calls = 0
complexity_limit = initial_complexity
instantiator = Instantiator(static_externals, evaluations)
num_calls += process_stream_queue(instantiator,
store,
complexity_limit,
verbose=verbose)
while not store.is_terminated() and (num_iterations < max_iterations) and (
complexity_limit <= max_complexity):
num_iterations += 1
print(
'Iteration: {} | Complexity: {} | Calls: {} | Evaluations: {} | Solved: {} | Cost: {:.3f} | '
'Search Time: {:.3f} | Sample Time: {:.3f} | Time: {:.3f}'.format(
num_iterations, complexity_limit, num_calls, len(evaluations),
store.has_solution(), store.best_cost, store.search_time,
store.sample_time, store.elapsed_time()))
plan, cost = solve_finite(evaluations,
goal_expression,
domain,
max_cost=min(store.best_cost,
constraints.max_cost),
**search_kwargs)
if is_plan(plan):
store.add_plan(plan, cost)
if not instantiator:
break
if complexity_step is None:
# TODO: option to select the next k-smallest complexities
complexity_limit = instantiator.min_complexity()
else:
complexity_limit += complexity_step
num_calls += process_stream_queue(instantiator,
store,
complexity_limit,
verbose=verbose)
#retrace_stream_plan(store, domain, goal_expression)
#print('Final queue size: {}'.format(len(instantiator)))
summary = store.export_summary()
summary.update({
'iterations': num_iterations,
'complexity': complexity_limit,
})
print('Summary: {}'.format(str_from_object(
summary, ndigits=3))) # TODO: return the summary
if UPDATE_STATISTICS:
write_stream_statistics(externals, verbose)
return store.extract_solution()
def collect_stir(world, num_beads=100):
arm = LEFT_ARM
# TODO: randomize geometries for stirrer
spoon_name = create_name(
'grey_spoon', 1) # green_spoon | grey_spoon | orange_spoon | stirrer
bowl_name = create_name('whitebowl', 1)
scale_name = create_name('onyx_scale', 1)
item_ranges = {
spoon_name:
InitialRanges(
width_range=(1., 1.),
height_range=(1., 1.),
mass_range=(1., 1.),
pose2d_range=([0.3, 0.5, -np.pi / 2], [0.3, 0.5, -np.pi / 2]),
surface=TABLE_NAME,
),
bowl_name:
InitialRanges(
width_range=(0.75, 1.25),
height_range=(0.75, 1.25),
mass_range=(1., 1.),
pose2d_range=([0.5, -0.05, -np.pi], [0.6, 0.05,
np.pi]), # x, y, theta
surface=scale_name,
),
}
# TODO: make a sandbox on the table to contain the beads
##################################################
#alpha = 0.75
alpha = 1
bead_colors = [
(1, 0, 0, alpha),
(0, 0, 1, alpha),
]
beads_fraction = random.uniform(0.75, 1.25)
print('Beads fraction:', beads_fraction)
bead_radius = sample_norm(mu=0.006, sigma=0.001, lower=0.004) # 0.007
print('Bead radius:', bead_radius)
# TODO: check collisions/feasibility when sampling
# TODO: grasps on the blue cup seem off for some reason...
with ClientSaver(world.client):
#dump_body(world.robot)
world.perception.add_surface('sandbox', TABLE_POSE)
world.perception.add_surface(scale_name, TABLE_POSE)
create_table_bodies(world, item_ranges)
#gripper = create_gripper(world.robot, LEFT_ARM)
#set_point(gripper, (1, 0, 1))
#wait_for_user()
bowl_body = world.get_body(bowl_name)
update_world(world, target_body=bowl_body)
init_holding = hold_item(world, arm, spoon_name)
if init_holding is None:
return INFEASIBLE
parameters_from_name = randomize_dynamics(
world) # TODO: parameters_from_name['bead']
_, (d, h) = approximate_as_cylinder(bowl_body)
bowl_area = np.pi * (d / 2.)**2
print('Bowl area:', bowl_area)
bead_area = np.pi * bead_radius**2
print('Bead area:', bead_area)
num_beads = int(np.ceil(beads_fraction * bowl_area / bead_area))
print('Num beads:', num_beads)
num_per_color = int(num_beads / len(bead_colors))
# TODO: randomize bead physics
beads_per_color = [
fill_with_beads(
world, bowl_name,
create_beads(num_beads,
bead_radius,
uniform_color=color,
parameters={})) for color in bead_colors
]
world.initial_beads.update(
{bead: bowl_body
for beads in beads_per_color for bead in beads})
if any(len(beads) != num_per_color for beads in beads_per_color):
return INFEASIBLE
#wait_for_user()
init = [('Contains', bowl_name, COFFEE)]
goal = [('Mixed', bowl_name)]
skeleton = [
('move-arm', [arm, X, X, X]),
('stir', [arm, bowl_name, X, spoon_name, X, X, X, X]),
#('move-arm', [arm, X, X, X]),
]
constraints = PlanConstraints(skeletons=[skeleton], exact=True)
task = Task(init=init,
goal=goal,
arms=[arm],
init_holding=init_holding,
reset_arms=False,
constraints=constraints) # Reset arm to clear the scene
# TODO: constrain the plan skeleton within the task
feature = get_stir_feature(world, bowl_name, spoon_name)
##################################################
# table_body = world.get_body(TABLE_NAME)
# dump_body(table_body)
# joint = 0
# p.enableJointForceTorqueSensor(table_body, joint, enableSensor=1, physicsClientId=world.client)
# stabilize(world)
# reaction_force = get_joint_reaction_force(table_body, joint)
# print(np.array(reaction_force[:3])/ GRAVITY)
perception = world.perception
initial_pose = perception.get_pose(bowl_name)
bowl_body = perception.get_body(bowl_name)
scale_body = perception.get_body(scale_name)
with ClientSaver(world.client):
initial_distance = compute_dispersion(bowl_body, beads_per_color)
initial_mass = read_mass(scale_body)
print(initial_mass)
def score_fn(plan):
assert plan is not None
with ClientSaver(world.client):
rgb_image = take_image(world, bowl_body, beads_per_color)
values = score_image(rgb_image, bead_colors, beads_per_color)
final_pose = perception.get_pose(bowl_name)
point_distance = get_distance(point_from_pose(initial_pose),
point_from_pose(final_pose)) #, norm=2)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format(
point_distance, quat_distance))
with ClientSaver(world.client):
all_beads = list(flatten(beads_per_color))
bowl_beads = get_contained_beads(bowl_body, all_beads)
fraction_bowl = float(
len(bowl_beads)) / len(all_beads) if all_beads else 0
print('In Bowl: {}'.format(fraction_bowl))
with ClientSaver(world.client):
final_dispersion = compute_dispersion(bowl_body, beads_per_color)
print('Initial Dispersion: {:.3f} | Final Dispersion {:.3f}'.format(
initial_distance, final_dispersion))
score = {
'bowl_translation': point_distance,
'bowl_rotation': quat_distance,
'fraction_in_bowl': fraction_bowl,
'initial_dispersion': initial_distance,
'final_dispersion': final_dispersion,
'num_beads': len(all_beads), # Beads per color
DYNAMICS: parameters_from_name,
}
# TODO: include time required for stirring
# TODO: change in dispersion
#wait_for_user()
#_, args = find_unique(lambda a: a[0] == 'stir', plan)
#control = args[-1]
return score
return task, feature, score_fn
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-a', '--algorithm', default='focused', help='Specifies the algorithm')
parser.add_argument('-s', '--skeleton', action='store_true', help='Enforces skeleton plan constraints')
tamp_problem, args = initialize(parser)
colors = dict(zip(sorted(tamp_problem.initial.block_poses.keys()), COLORS))
viewer = ContinuousTMPViewer(SUCTION_HEIGHT, tamp_problem.regions, title='Continuous TAMP')
draw_state(viewer, tamp_problem.initial, colors)
[robot] = tamp_problem.initial.robot_confs
blocks = sorted(tamp_problem.initial.block_poses)
regions = sorted(set(tamp_problem.regions) - {GROUND_NAME})
print(robot)
print(blocks)
print(regions)
print(tamp_problem)
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
stream_map = DEBUG # TODO: reuse same streams
stream_info = {
't-region': StreamInfo(eager=False, p_success=0), # bound_fn is None
}
streams_from_problem = {}
for order in permutations(blocks):
for region in regions: # Objects could also have separate goals
print(SEPARATOR)
print('Block order:', order)
print('Goal region:', region)
#tamp_problem = dual(n_blocks=args.number, goal_regions=[region])
tamp_problem.goal_regions.update({block: region for block in order})
init, goal = create_problem(tamp_problem)
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
dump_pddlstream(problem)
skeleton = create_skeleton(robot, order, home=tamp_problem.goal_conf is not None)
print(skeleton)
skeletons = [skeleton]
#skeletons = None
constraints = PlanConstraints(skeletons=skeletons, exact=True)
solution = solve_focused(problem, constraints=constraints, stream_info=stream_info,
planner='max-astar', max_planner_time=10, debug=False,
max_time=args.max_time, verbose=True,
unit_costs=True, unit_efforts=True, effort_weight=1)
print_solution(solution)
plan, cost, evaluations = solution
assert plan is not None
# TODO: params not sufficient if no output stream or output not in plan
streams = set()
for name, params in plan:
for param in params:
value = get_stream_value(param)
if isinstance(value, StreamValue):
# TODO: propagate recursively
streams.add((value.stream, value.inputs))
streams_from_problem[order, region] = streams
#print(streams)
#user_input()
print(SEPARATOR)
dump_statistics()
print(SEPARATOR)
best_problems, best_p = None, -INF
for problems in combinations(streams_from_problem, r=2): # Make r a parameter
stream_plans = [streams_from_problem[problem] for problem in problems]
intersection = set.intersection(*stream_plans)
p = p_disjunction(stream_plans, STATS_PER_STREAM)
assert 0 <= p <= 1
print('Problems: {} | Intersection: {} | p={:.3f}'.format(problems, len(intersection), p)) #, intersection)
if p > best_p:
best_problems, best_p = problems, p
print('\nBest: {} (p={:.3f})'.format(best_problems, best_p))
def plan_actions(world,
use_constraints=False,
unit_costs=True,
max_time=300,
verbose=True,
**kwargs):
# TODO: return multiple grasps instead of one
pr = cProfile.Profile()
pr.enable()
with ClientSaver(world.client):
# TODO: be careful about the table distance
table_pose = get_pose(world.get_body(world.get_table()))
torso_pose = get_link_pose(
world.robot, link_from_name(world.robot, 'torso_lift_link'))
torso_in_table = multiply(invert(table_pose), torso_pose)
# Torso wrt table: [-0.6, 0.0, 0.33]
print('Torso wrt table:',
np.array(point_from_pose(torso_in_table)).round(3).tolist())
#wait_for_interrupt()
problem = get_pddlstream(world, **kwargs)
p_success = 1e-2
eager = True
stream_info = {
'ControlPoseCollision': FunctionInfo(p_success=p_success,
eager=eager),
'ControlConfCollision': FunctionInfo(p_success=p_success,
eager=eager),
'PosePoseCollision': FunctionInfo(p_success=p_success,
eager=eager),
'ConfConfCollision': FunctionInfo(p_success=p_success,
eager=eager),
'test-reachable': StreamInfo(p_success=0, eager=True),
# TODO: these should automatically be last...
'sample-motion': StreamInfo(p_success=1, overhead=100),
}
# TODO: RuntimeError: Preimage fact ('order', n0, t0) is not achievable!
constraints = world.task.constraints if use_constraints else PlanConstraints(
)
solution = solve_focused(
problem,
planner='ff-wastar1',
max_time=max_time,
unit_costs=unit_costs,
unit_efforts=True,
effort_weight=1,
stream_info=stream_info,
# TODO: bug when max_skeletons=None and effort_weight != None
max_skeletons=None,
bind=True,
max_failures=0,
search_sample_ratio=0,
constraints=constraints,
verbose=verbose,
debug=False)
#solution = solve_incremental(problem, unit_costs=unit_costs, verbose=True)
print_solution(solution)
plan, cost, evaluations = solution
pr.disable()
if verbose:
pstats.Stats(pr).sort_stats('tottime').print_stats(
10) # cumtime | tottime
return plan
def collect_pour(world,
bowl_type=None,
cup_type=None,
randomize=True,
**kwargs):
arm = LEFT_ARM
bowl_type = random.choice(POUR_BOWLS) if bowl_type is None else bowl_type
cup_type = random.choice(POUR_CUPS) if cup_type is None else cup_type
# TODO: could directly randomize base_diameter and top_diameter
scale = int(randomize)
cup_name = create_name(cup_type, 1)
bowl_name = create_name(bowl_type, 1)
item_ranges = {
cup_name:
InitialRanges(
width_range=interval(extent=0.2 *
scale), # the cups are already fairly small
#height_range=(0.9, 1.1),
height_range=(1.0, 1.2), # Difficult to grasp when this shrinks
mass_range=interval(extent=0.2 * scale),
pose2d_range=([0.5, 0.3, -np.pi], [0.5, 0.3,
np.pi]), # x, y, theta
),
bowl_name:
InitialRanges(
width_range=interval(extent=0.4 * scale),
height_range=interval(extent=0.4 * scale),
mass_range=interval(extent=0.4 * scale),
pose2d_range=([0.5, 0.0, -np.pi], [0.5, 0.0,
np.pi]), # x, y, theta
),
}
if 'item_ranges' in kwargs:
for item in kwargs['item_ranges']:
item_ranges[item] = kwargs['item_ranges'][item]
#dump_ranges(POUR_CUPS, item_ranges[cup_name])
#dump_ranges(POUR_BOWLS, item_ranges[bowl_name])
#for name, limits in item_ranges.items():
# lower, upper = aabb_from_points(read_obj(get_body_obj(name)))
# extents = upper - lower
# print(name, 'width', (extents[0]*np.array(limits.width_range)).tolist())
# print(name, 'height', (extents[2]*np.array(limits.height_range)).tolist())
# TODO: check collisions/feasibility when sampling
cup_fraction = random.uniform(0.75, 1.0)
print('Cup fraction:', cup_fraction)
bowl_fraction = random.uniform(1.0, 1.0)
print('Bowl fraction:', bowl_fraction)
bead_radius = sample_norm(mu=0.006, sigma=0.001 * scale, lower=0.004)
print('Bead radius:', bead_radius)
num_beads = 100
with ClientSaver(world.client):
create_table_bodies(world, item_ranges, randomize=randomize)
bowl_body = world.get_body(bowl_name)
cup_body = world.get_body(cup_name)
update_world(world, bowl_body)
if body_pair_collision(cup_body, bowl_body):
# TODO: automatically try all pairs of bodies
return INFEASIBLE
if not has_grasp(world, cup_name):
return INFEASIBLE
# TODO: flatten and only store bowl vs cup
parameters_from_name = randomize_dynamics(world, randomize=randomize)
parameters_from_name['bead'] = sample_bead_parameters(
) # bead restitution is the most important
with LockRenderer():
all_beads = create_beads(num_beads,
bead_radius,
parameters=parameters_from_name['bead'])
bowl_beads = fill_with_beads(world,
bowl_name,
all_beads,
reset_contained=True,
height_fraction=bowl_fraction)
init_beads = fill_with_beads(world,
cup_name,
bowl_beads,
reset_contained=False,
height_fraction=cup_fraction)
#wait_for_user()
init_mass = sum(map(get_mass, init_beads))
print('Init beads: {} | Init mass: {:.3f}'.format(
len(init_beads), init_mass))
if len(init_beads) < MIN_BEADS:
return INFEASIBLE
world.initial_beads.update({bead: cup_body for bead in init_beads})
latent = {
'num_beads': len(init_beads),
'total_mass': init_mass,
'bead_radius': bead_radius,
DYNAMICS: parameters_from_name,
}
init = [('Contains', cup_name, COFFEE)]
goal = [('Contains', bowl_name, COFFEE)]
skeleton = [
('move-arm', [arm, X, X, X]),
('pick', [arm, cup_name, X, X, X, X, X]),
('move-arm', [arm, X, X, X]),
('pour', [arm, bowl_name, X, cup_name, X, COFFEE, X, X, X]),
]
constraints = PlanConstraints(skeletons=[skeleton], exact=True)
task = Task(init=init, goal=goal, arms=[arm],
constraints=constraints) #, init_holding=init_holding)
##################################################
feature = get_pour_feature(world, bowl_name, cup_name)
#feature['ranges'] = POUR_FEATURE_RANGES
initial_pose = world.get_pose(bowl_name)
def score_fn(plan):
assert plan is not None
final_pose = world.get_pose(bowl_name)
point_distance = get_distance(point_from_pose(initial_pose),
point_from_pose(final_pose)) #, norm=2)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format(
point_distance, quat_distance))
with ClientSaver(world.client):
# TODO: lift the bowl up (with particles around) to prevent scale detections
final_bowl_beads = get_contained_beads(bowl_body, init_beads)
fraction_bowl = safe_ratio(len(final_bowl_beads),
len(init_beads),
undefined=0)
mass_in_bowl = sum(map(get_mass, final_bowl_beads))
final_cup_beads = get_contained_beads(cup_body, init_beads)
fraction_cup = safe_ratio(len(final_cup_beads),
len(init_beads),
undefined=0)
mass_in_cup = sum(map(get_mass, final_cup_beads))
print('In Bowl: {} | In Cup: {}'.format(fraction_bowl, fraction_cup))
score = {
# Displacements
'bowl_translation': point_distance,
'bowl_rotation': quat_distance,
# Masses
'mass_in_bowl': mass_in_bowl,
'mass_in_cup': mass_in_cup,
# Counts
'bowl_beads': len(final_bowl_beads),
'cup_beads': len(final_cup_beads),
# Fractions
'fraction_in_bowl': fraction_bowl,
'fraction_in_cup': fraction_cup,
}
score.update(latent)
# TODO: store the cup path length to bias towards shorter paths
#_, args = find_unique(lambda a: a[0] == 'pour', plan)
#control = args[-1]
#feature = control['feature']
#parameter = control['parameter']
return score
return task, feature, score_fn
def solve_focused(problem, constraints=PlanConstraints(),
stream_info={}, action_info={}, synthesizers=[],
max_time=INF, max_iterations=INF, max_skeletons=INF,
unit_costs=False, success_cost=INF,
complexity_step=1,
unit_efforts=False, max_effort=INF, effort_weight=None,
reorder=True, search_sample_ratio=0,
visualize=False, verbose=True, **search_kwargs):
"""
Solves a PDDLStream problem by first hypothesizing stream outputs and then determining whether they exist
:param problem: a PDDLStream problem
:param constraints: PlanConstraints on the set of legal solutions
:param stream_info: a dictionary from stream name to StreamInfo altering how individual streams are handled
:param action_info: a dictionary from stream name to ActionInfo for planning and execution
:param synthesizers: a list of StreamSynthesizer objects
:param max_time: the maximum amount of time to apply streams
:param max_iterations: the maximum number of search iterations
:param max_iterations: the maximum number of plan skeletons to consider
:param unit_costs: use unit action costs rather than numeric costs
:param success_cost: an exclusive (strict) upper bound on plan cost to terminate
:param unit_efforts: use unit stream efforts rather than estimated numeric efforts
:param complexity_step: the increase in the effort limit after each failure
:param max_effort: the maximum amount of effort to consider for streams
:param effort_weight: a multiplier for stream effort compared to action costs
:param reorder: if True, stream plans are reordered to minimize the expected sampling overhead
:param search_sample_ratio: the desired ratio of search time / sample time
:param visualize: if True, it draws the constraint network and stream plan as a graphviz file
:param verbose: if True, this prints the result of each stream application
:param search_kwargs: keyword args for the search subroutine
:return: a tuple (plan, cost, evaluations) where plan is a sequence of actions
(or None), cost is the cost of the plan, and evaluations is init but expanded
using stream applications
"""
# TODO: select whether to search or sample based on expected success rates
# TODO: no optimizers during search with relaxed_stream_plan
num_iterations = search_time = sample_time = eager_calls = 0
complexity_limit = float(INITIAL_COMPLEXITY)
eager_disabled = effort_weight is None # No point if no stream effort biasing
evaluations, goal_exp, domain, externals = parse_problem(
problem, stream_info=stream_info, constraints=constraints,
unit_costs=unit_costs, unit_efforts=unit_efforts)
store = SolutionStore(evaluations, max_time, success_cost, verbose)
full_action_info = get_action_info(action_info)
load_stream_statistics(externals + synthesizers)
if visualize and not has_pygraphviz():
visualize = False
print('Warning, visualize=True requires pygraphviz. Setting visualize=False')
if visualize:
reset_visualizations()
streams, functions, negative = partition_externals(externals, verbose=verbose)
eager_externals = list(filter(lambda e: e.info.eager, externals))
skeleton_queue = SkeletonQueue(store, goal_exp, domain)
disabled = set()
while (not store.is_terminated()) and (num_iterations < max_iterations):
start_time = time.time()
num_iterations += 1
eager_instantiator = Instantiator(eager_externals, evaluations) # Only update after an increase?
if eager_disabled:
push_disabled(eager_instantiator, disabled)
eager_calls += process_stream_queue(eager_instantiator, store, complexity_limit=complexity_limit, verbose=verbose)
print('\nIteration: {} | Complexity: {} | Skeletons: {} | Skeleton Queue: {} | Disabled: {} | Evaluations: {} | '
'Eager Calls: {} | Cost: {:.3f} | Search Time: {:.3f} | Sample Time: {:.3f} | Total Time: {:.3f}'.format(
num_iterations, complexity_limit, len(skeleton_queue.skeletons), len(skeleton_queue), len(disabled),
len(evaluations), eager_calls, store.best_cost, search_time, sample_time, store.elapsed_time()))
optimistic_solve_fn = get_optimistic_solve_fn(goal_exp, domain, negative,
max_cost=min(store.best_cost, constraints.max_cost),
unit_efforts=unit_efforts, max_effort=max_effort,
effort_weight=effort_weight, **search_kwargs)
if (max_skeletons is not None) and (len(skeleton_queue.skeletons) < max_skeletons):
combined_plan, cost = iterative_plan_streams(evaluations, externals, optimistic_solve_fn, complexity_limit,
unit_efforts=unit_efforts, max_effort=max_effort)
else:
combined_plan, cost = INFEASIBLE, INF
if action_info:
combined_plan = reorder_combined_plan(evaluations, combined_plan, full_action_info, domain)
print('Combined plan: {}'.format(combined_plan))
stream_plan, action_plan = separate_plan(combined_plan, full_action_info)
#stream_plan = replan_with_optimizers(evaluations, stream_plan, domain, externals)
stream_plan = combine_optimizers(evaluations, stream_plan)
#stream_plan = get_synthetic_stream_plan(stream_plan, # evaluations
# [s for s in synthesizers if not s.post_only])
if reorder:
stream_plan = reorder_stream_plan(stream_plan) # This may be redundant when using reorder_combined_plan
print('Stream plan ({}, {:.3f}): {}\nAction plan ({}, {:.3f}): {}'.format(
get_length(stream_plan), compute_plan_effort(stream_plan), stream_plan,
get_length(action_plan), cost, str_from_plan(action_plan)))
if is_plan(stream_plan) and visualize:
log_plans(stream_plan, action_plan, num_iterations)
create_visualizations(evaluations, stream_plan, num_iterations)
search_time += elapsed_time(start_time)
if (stream_plan is INFEASIBLE) and (not eager_instantiator) and (not skeleton_queue) and (not disabled):
break
start_time = time.time()
if not is_plan(stream_plan):
complexity_limit += complexity_step
if not eager_disabled:
reenable_disabled(evaluations, domain, disabled)
elif not stream_plan:
store.add_plan(action_plan, cost)
if max_skeletons is None:
process_stream_plan(store, domain, disabled, stream_plan)
else:
allocated_sample_time = (search_sample_ratio * search_time) - sample_time
skeleton_queue.process(stream_plan, action_plan, cost, complexity_limit, allocated_sample_time)
sample_time += elapsed_time(start_time)
write_stream_statistics(externals + synthesizers, verbose)
return store.extract_solution()
def collect_scoop(world):
arm = LEFT_ARM
spoon_name = create_name(random.choice(SCOOP_SPOONS), 1)
bowl_name = create_name(random.choice(SCOOP_BOWLS), 1)
item_ranges = {
spoon_name:
InitialRanges(
width_range=(1., 1.),
height_range=(1., 1.),
mass_range=(1., 1.), # TODO: randomize density?
pose2d_range=([0.3, 0.5, -np.pi / 2], [0.3, 0.5, -np.pi / 2]),
),
bowl_name:
InitialRanges(
width_range=(0.8, 1.2),
height_range=(0.8, 1.2),
mass_range=(0.8, 1.2),
pose2d_range=([0.5, 0.0, -np.pi], [0.5, 0.0,
np.pi]), # x, y, theta
),
}
#dump_ranges(SCOOP_SPOONS, None)
dump_ranges(SCOOP_BOWLS, item_ranges[bowl_name])
##################################################
# TODO: check collisions/feasibility when sampling
#bowl_fraction = random.uniform(0.5, 0.75)
bowl_fraction = random.uniform(0.75, 0.75)
print('Bowl fraction:', bowl_fraction)
#bead_radius = sample_norm(mu=0.005, sigma=0.0005, lower=0.004, upper=0.007)
bead_radius = 0.005
print('Bead radius:', bead_radius) # Chickpeas have a 1cm diameter
max_beads = 250
# TODO: could give the bowl infinite mass
with ClientSaver(world.client):
create_table_bodies(world, item_ranges)
bowl_body = world.get_body(bowl_name)
update_world(world, target_body=bowl_body)
parameters_from_name = randomize_dynamics(world)
parameters_from_name['bead'] = sample_bead_parameters()
with LockRenderer():
all_beads = create_beads(max_beads,
bead_radius,
parameters=parameters_from_name['bead'])
spoon_capacity = estimate_spoon_capacity(world, spoon_name, all_beads)
print('{} | Capacity: {} | Mass: {:.3f}'.format(
spoon_name, spoon_capacity, 0.0))
if spoon_capacity < MIN_CAPACITY:
return INFEASIBLE
init_holding = hold_item(world, arm, spoon_name)
if init_holding is None:
return INFEASIBLE
# TODO: relate to the diameter of the spoon head. Ensure fraction above this level
init_beads = fill_with_beads(world,
bowl_name,
all_beads,
reset_contained=False,
height_fraction=bowl_fraction)
#wait_for_user()
masses = list(map(get_mass, init_beads))
mean_mass = np.average(masses)
init_mass = sum(masses)
print('Init beads: {} | Init mass: {:.3f} | Mean mass: {:.3f}'.format(
len(init_beads), init_mass, mean_mass))
if len(init_beads) < 2 * spoon_capacity:
return INFEASIBLE
world.initial_beads.update({bead: bowl_body for bead in init_beads})
init = [('Contains', bowl_name, COFFEE)]
goal = [('Contains', spoon_name, COFFEE)]
skeleton = [
('move-arm', [arm, X, X, X]),
('scoop', [arm, bowl_name, X, spoon_name, X, COFFEE, X, X, X]),
]
constraints = PlanConstraints(skeletons=[skeleton], exact=True)
task = Task(init=init,
goal=goal,
arms=[arm],
init_holding=init_holding,
constraints=constraints)
##################################################
feature = get_scoop_feature(world, bowl_name, spoon_name)
initial_pose = world.get_pose(bowl_name)
def score_fn(plan):
assert plan is not None
final_pose = world.get_pose(bowl_name)
point_distance = get_distance(point_from_pose(initial_pose),
point_from_pose(final_pose)) #, norm=2)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format(
point_distance, quat_distance))
with ClientSaver(world.client):
bowl_beads = get_contained_beads(bowl_body, init_beads)
fraction_bowl = float(
len(bowl_beads)) / len(init_beads) if init_beads else 0
mass_in_bowl = sum(map(get_mass, bowl_beads))
spoon_beads = get_contained_beads(world.get_body(spoon_name),
init_beads)
fraction_spoon = float(
len(spoon_beads)) / len(init_beads) if init_beads else 0
mass_in_spoon = sum(map(get_mass, spoon_beads))
print('In Bowl: {:.3f} | In Spoon: {:.3f}'.format(
fraction_bowl, fraction_spoon))
# TODO: measure change in roll/pitch
# TODO: could make latent parameters field
score = {
# Displacements
'bowl_translation': point_distance,
'bowl_rotation': quat_distance,
# Masses
'total_mass': init_mass,
'mass_in_bowl': mass_in_bowl,
'mass_in_spoon': mass_in_spoon,
'spoon_mass_capacity':
(init_mass / len(init_beads)) * spoon_capacity,
# Counts
'num_beads': len(init_beads),
'bowl_beads': len(bowl_beads),
'spoon_beads': len(spoon_beads),
'spoon_capacity': spoon_capacity,
# Fractions
'fraction_in_bowl': fraction_bowl,
'fraction_in_spoon': fraction_spoon,
# Latent
'bead_radius': bead_radius,
DYNAMICS: parameters_from_name
}
fraction_filled = float(score['spoon_beads']) / score['spoon_capacity']
spilled_beads = score['num_beads'] - (score['bowl_beads'] +
score['spoon_beads'])
fraction_spilled = float(spilled_beads) / score['num_beads']
print('Fraction Filled: {} | Fraction Spilled: {}'.format(
fraction_filled, fraction_spilled))
#_, args = find_unique(lambda a: a[0] == 'scoop', plan)
#control = args[-1]
return score
return task, feature, score_fn
def collect_push(world):
arm = LEFT_ARM
block_name = create_name('purpleblock', 1)
item_ranges = {
block_name:
InitialRanges(
width_range=(0.75, 1.25),
height_range=(0.75, 1.25),
mass_range=(1., 1.),
pose2d_range=POSE2D_RANGE, # x, y, theta
),
}
##################################################
# TODO: check collisions/feasibility when sampling
# TODO: grasps on the blue cup seem off for some reason...
with ClientSaver(world.client):
create_table_bodies(world, item_ranges)
update_world(world, world.get_body(TABLE_NAME))
parameters_from_name = randomize_dynamics(world)
stabilize(world)
lower, upper = item_ranges[block_name].pose2d_range
goal_pos2d = np.random.uniform(lower, upper)[:2]
draw_push_goal(world, block_name, goal_pos2d)
initial_pose = world.perception.get_pose(block_name)
feature = get_push_feature(world, arm, block_name, initial_pose,
goal_pos2d)
init = [('CanPush', block_name, goal_pos2d)]
goal = [('InRegion', block_name, goal_pos2d)]
skeleton = [
('move-arm', [arm, X, X, X]),
('push', [arm, block_name, X, X, X, X, X]),
]
constraints = PlanConstraints(skeletons=[skeleton], exact=True)
task = Task(init=init, goal=goal, arms=[arm], constraints=constraints)
##################################################
def score_fn(plan):
assert plan is not None
initial_distance = get_distance(
point_from_pose(initial_pose)[:2], goal_pos2d)
final_pose = world.perception.get_pose(block_name)
final_distance = get_distance(
point_from_pose(final_pose)[:2], goal_pos2d)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print(
'Initial: {:.5f} m | Final: {:.5f} | Rotation: {:.5f} rads'.format(
initial_distance, final_distance, quat_distance))
# TODO: compare orientation to final predicted orientation
# TODO: record simulation time in the event that the controller gets stuck
score = {
'initial_distance': initial_distance,
'final_distance': final_distance,
'rotation': quat_distance,
DYNAMICS: parameters_from_name,
}
#_, args = find_unique(lambda a: a[0] == 'push', plan)
#control = args[-1]
return score
return task, feature, score_fn
def solve_abstract(problem,
constraints=PlanConstraints(),
stream_info={},
replan_actions=set(),
unit_costs=False,
success_cost=INF,
max_time=INF,
max_iterations=INF,
max_memory=INF,
initial_complexity=0,
complexity_step=1,
max_complexity=INF,
max_skeletons=INF,
search_sample_ratio=0,
bind=True,
max_failures=0,
unit_efforts=False,
max_effort=INF,
effort_weight=None,
reorder=True,
visualize=False,
verbose=True,
**search_kwargs):
"""
Solves a PDDLStream problem by first planning with optimistic stream outputs and then querying streams
:param problem: a PDDLStream problem
:param constraints: PlanConstraints on the set of legal solutions
:param stream_info: a dictionary from stream name to StreamInfo altering how individual streams are handled
:param replan_actions: the actions declared to induce replanning for the purpose of deferred stream evaluation
:param unit_costs: use unit action costs rather than numeric costs
:param success_cost: the exclusive (strict) upper bound on plan cost to successfully terminate
:param max_time: the maximum runtime
:param max_iterations: the maximum number of search iterations
:param max_memory: the maximum amount of memory
:param initial_complexity: the initial stream complexity limit
:param complexity_step: the increase in the stream complexity limit per iteration
:param max_complexity: the maximum stream complexity limit
:param max_skeletons: the maximum number of plan skeletons (max_skeletons=None indicates not adaptive)
:param search_sample_ratio: the desired ratio of sample time / search time when max_skeletons!=None
:param bind: if True, propagates parameter bindings when max_skeletons=None
:param max_failures: the maximum number of stream failures before switching phases when max_skeletons=None
:param unit_efforts: use unit stream efforts rather than estimated numeric efforts
:param max_effort: the maximum amount of stream effort
:param effort_weight: a multiplier for stream effort compared to action costs
:param reorder: if True, reorder stream plans to minimize the expected sampling overhead
:param visualize: if True, draw the constraint network and stream plan as a graphviz file
:param verbose: if True, print the result of each stream application
:param search_kwargs: keyword args for the search subroutine
:return: a tuple (plan, cost, evaluations) where plan is a sequence of actions
(or None), cost is the cost of the plan (INF if no plan), and evaluations is init expanded
using stream applications
"""
# TODO: select whether to search or sample based on expected success rates
# TODO: no optimizers during search with relaxed_stream_plan
# TODO: locally optimize only after a solution is identified
# TODO: replan with a better search algorithm after feasible
# TODO: change the search algorithm and unit costs based on the best cost
use_skeletons = (max_skeletons is not None)
#assert implies(use_skeletons, search_sample_ratio > 0)
eager_disabled = (effort_weight is None
) # No point if no stream effort biasing
num_iterations = eager_calls = 0
complexity_limit = initial_complexity
evaluations, goal_exp, domain, externals = parse_problem(
problem,
stream_info=stream_info,
constraints=constraints,
unit_costs=unit_costs,
unit_efforts=unit_efforts)
identify_non_producers(externals)
enforce_simultaneous(domain, externals)
compile_fluent_streams(domain, externals)
# TODO: make effort_weight be a function of the current cost
# if (effort_weight is None) and not has_costs(domain):
# effort_weight = 1
load_stream_statistics(externals)
if visualize and not has_pygraphviz():
visualize = False
print(
'Warning, visualize=True requires pygraphviz. Setting visualize=False'
)
if visualize:
reset_visualizations()
streams, functions, negative, optimizers = partition_externals(
externals, verbose=verbose)
eager_externals = list(filter(lambda e: e.info.eager, externals))
positive_externals = streams + functions + optimizers
has_optimizers = bool(optimizers) # TODO: deprecate
assert implies(has_optimizers, use_skeletons)
################
store = SolutionStore(evaluations,
max_time,
success_cost,
verbose,
max_memory=max_memory)
skeleton_queue = SkeletonQueue(store, domain, disable=not has_optimizers)
disabled = set() # Max skeletons after a solution
while (not store.is_terminated()) and (
num_iterations < max_iterations) and (complexity_limit <=
max_complexity):
num_iterations += 1
eager_instantiator = Instantiator(
eager_externals, evaluations) # Only update after an increase?
if eager_disabled:
push_disabled(eager_instantiator, disabled)
if eager_externals:
eager_calls += process_stream_queue(
eager_instantiator,
store,
complexity_limit=complexity_limit,
verbose=verbose)
################
print(
'\nIteration: {} | Complexity: {} | Skeletons: {} | Skeleton Queue: {} | Disabled: {} | Evaluations: {} | '
'Eager Calls: {} | Cost: {:.3f} | Search Time: {:.3f} | Sample Time: {:.3f} | Total Time: {:.3f}'
.format(num_iterations, complexity_limit,
len(skeleton_queue.skeletons), len(skeleton_queue),
len(disabled), len(evaluations), eager_calls,
store.best_cost, store.search_time, store.sample_time,
store.elapsed_time()))
optimistic_solve_fn = get_optimistic_solve_fn(
goal_exp,
domain,
negative,
replan_actions=replan_actions,
reachieve=use_skeletons,
max_cost=min(store.best_cost, constraints.max_cost),
max_effort=max_effort,
effort_weight=effort_weight,
**search_kwargs)
# TODO: just set unit effort for each stream beforehand
if (max_skeletons is None) or (len(skeleton_queue.skeletons) <
max_skeletons):
disabled_axioms = create_disabled_axioms(
skeleton_queue) if has_optimizers else []
if disabled_axioms:
domain.axioms.extend(disabled_axioms)
stream_plan, opt_plan, cost = iterative_plan_streams(
evaluations,
positive_externals,
optimistic_solve_fn,
complexity_limit,
max_effort=max_effort)
for axiom in disabled_axioms:
domain.axioms.remove(axiom)
else:
stream_plan, opt_plan, cost = OptSolution(
INFEASIBLE, INFEASIBLE, INF) # TODO: apply elsewhere
################
#stream_plan = replan_with_optimizers(evaluations, stream_plan, domain, externals) or stream_plan
stream_plan = combine_optimizers(evaluations, stream_plan)
#stream_plan = get_synthetic_stream_plan(stream_plan, # evaluations
# [s for s in synthesizers if not s.post_only])
#stream_plan = recover_optimistic_outputs(stream_plan)
if reorder:
# TODO: this blows up memory wise for long stream plans
stream_plan = reorder_stream_plan(store, stream_plan)
num_optimistic = sum(r.optimistic
for r in stream_plan) if stream_plan else 0
action_plan = opt_plan.action_plan if is_plan(opt_plan) else opt_plan
print('Stream plan ({}, {}, {:.3f}): {}\nAction plan ({}, {:.3f}): {}'.
format(get_length(stream_plan), num_optimistic,
compute_plan_effort(stream_plan), stream_plan,
get_length(action_plan), cost,
str_from_plan(action_plan)))
if is_plan(stream_plan) and visualize:
log_plans(stream_plan, action_plan, num_iterations)
create_visualizations(evaluations, stream_plan, num_iterations)
################
if (stream_plan is INFEASIBLE) and (not eager_instantiator) and (
not skeleton_queue) and (not disabled):
break
if not is_plan(stream_plan):
print('No plan: increasing complexity from {} to {}'.format(
complexity_limit, complexity_limit + complexity_step))
complexity_limit += complexity_step
if not eager_disabled:
reenable_disabled(evaluations, domain, disabled)
#print(stream_plan_complexity(evaluations, stream_plan))
if not use_skeletons:
process_stream_plan(store,
domain,
disabled,
stream_plan,
opt_plan,
cost,
bind=bind,
max_failures=max_failures)
continue
################
#optimizer_plan = replan_with_optimizers(evaluations, stream_plan, domain, optimizers)
optimizer_plan = None
if optimizer_plan is not None:
# TODO: post process a bound plan
print('Optimizer plan ({}, {:.3f}): {}'.format(
get_length(optimizer_plan),
compute_plan_effort(optimizer_plan), optimizer_plan))
skeleton_queue.new_skeleton(optimizer_plan, opt_plan, cost)
allocated_sample_time = (search_sample_ratio * store.search_time) - store.sample_time \
if len(skeleton_queue.skeletons) <= max_skeletons else INF
if skeleton_queue.process(stream_plan, opt_plan, cost,
complexity_limit,
allocated_sample_time) is INFEASIBLE:
break
################
summary = store.export_summary()
summary.update({
'iterations': num_iterations,
'complexity': complexity_limit,
'skeletons': len(skeleton_queue.skeletons),
})
print('Summary: {}'.format(str_from_object(
summary, ndigits=3))) # TODO: return the summary
write_stream_statistics(externals, verbose)
return store.extract_solution()
def solve_focused(problem,
constraints=PlanConstraints(),
stream_info={},
replan_actions=set(),
max_time=INF,
max_iterations=INF,
initial_complexity=0,
complexity_step=1,
max_skeletons=INF,
bind=True,
max_failures=0,
unit_costs=False,
success_cost=INF,
unit_efforts=False,
max_effort=INF,
effort_weight=None,
reorder=True,
search_sample_ratio=0,
visualize=False,
verbose=True,
**search_kwargs):
"""
Solves a PDDLStream problem by first hypothesizing stream outputs and then determining whether they exist
:param problem: a PDDLStream problem
:param constraints: PlanConstraints on the set of legal solutions
:param stream_info: a dictionary from stream name to StreamInfo altering how individual streams are handled
:param max_time: the maximum amount of time to apply streams
:param max_iterations: the maximum number of search iterations
:param max_skeletons: the maximum number of plan skeletons to consider
:param unit_costs: use unit action costs rather than numeric costs
:param success_cost: an exclusive (strict) upper bound on plan cost to terminate
:param unit_efforts: use unit stream efforts rather than estimated numeric efforts
:param initial_complexity: the initial effort limit
:param complexity_step: the increase in the effort limit after each failure
:param max_effort: the maximum amount of effort to consider for streams
:param effort_weight: a multiplier for stream effort compared to action costs
:param reorder: if True, stream plans are reordered to minimize the expected sampling overhead
:param search_sample_ratio: the desired ratio of search time / sample time
:param visualize: if True, it draws the constraint network and stream plan as a graphviz file
:param verbose: if True, this prints the result of each stream application
:param search_kwargs: keyword args for the search subroutine
:return: a tuple (plan, cost, evaluations) where plan is a sequence of actions
(or None), cost is the cost of the plan, and evaluations is init but expanded
using stream applications
"""
# TODO: select whether to search or sample based on expected success rates
# TODO: no optimizers during search with relaxed_stream_plan
# TODO: locally optimize only after a solution is identified
# TODO: replan with a better search algorithm after feasible
num_iterations = search_time = sample_time = eager_calls = 0
complexity_limit = initial_complexity
# TODO: make effort_weight be a function of the current cost
# TODO: change the search algorithm and unit costs based on the best cost
eager_disabled = effort_weight is None # No point if no stream effort biasing
evaluations, goal_exp, domain, externals = parse_problem(
problem,
stream_info=stream_info,
constraints=constraints,
unit_costs=unit_costs,
unit_efforts=unit_efforts)
store = SolutionStore(evaluations, max_time, success_cost, verbose)
load_stream_statistics(externals)
if visualize and not has_pygraphviz():
visualize = False
print(
'Warning, visualize=True requires pygraphviz. Setting visualize=False'
)
if visualize:
reset_visualizations()
streams, functions, negative, optimizers = partition_externals(
externals, verbose=verbose)
eager_externals = list(filter(lambda e: e.info.eager, externals))
use_skeletons = max_skeletons is not None
has_optimizers = bool(optimizers)
assert implies(has_optimizers, use_skeletons)
skeleton_queue = SkeletonQueue(store, domain, disable=not has_optimizers)
disabled = set() # Max skeletons after a solution
while (not store.is_terminated()) and (num_iterations < max_iterations):
start_time = time.time()
num_iterations += 1
eager_instantiator = Instantiator(
eager_externals, evaluations) # Only update after an increase?
if eager_disabled:
push_disabled(eager_instantiator, disabled)
eager_calls += process_stream_queue(eager_instantiator,
store,
complexity_limit=complexity_limit,
verbose=verbose)
print(
'\nIteration: {} | Complexity: {} | Skeletons: {} | Skeleton Queue: {} | Disabled: {} | Evaluations: {} | '
'Eager Calls: {} | Cost: {:.3f} | Search Time: {:.3f} | Sample Time: {:.3f} | Total Time: {:.3f}'
.format(num_iterations, complexity_limit,
len(skeleton_queue.skeletons), len(skeleton_queue),
len(disabled), len(evaluations), eager_calls,
store.best_cost, search_time, sample_time,
store.elapsed_time()))
optimistic_solve_fn = get_optimistic_solve_fn(
goal_exp,
domain,
negative,
replan_actions=replan_actions,
reachieve=use_skeletons,
max_cost=min(store.best_cost, constraints.max_cost),
max_effort=max_effort,
effort_weight=effort_weight,
**search_kwargs)
# TODO: just set unit effort for each stream beforehand
if (max_skeletons is None) or (len(skeleton_queue.skeletons) <
max_skeletons):
disabled_axioms = create_disabled_axioms(
skeleton_queue) if has_optimizers else []
if disabled_axioms:
domain.axioms.extend(disabled_axioms)
stream_plan, action_plan, cost = iterative_plan_streams(
evaluations, (streams + functions + optimizers),
optimistic_solve_fn,
complexity_limit,
max_effort=max_effort)
for axiom in disabled_axioms:
domain.axioms.remove(axiom)
else:
stream_plan, action_plan, cost = INFEASIBLE, INFEASIBLE, INF
#stream_plan = replan_with_optimizers(evaluations, stream_plan, domain, externals) or stream_plan
stream_plan = combine_optimizers(evaluations, stream_plan)
#stream_plan = get_synthetic_stream_plan(stream_plan, # evaluations
# [s for s in synthesizers if not s.post_only])
if reorder:
stream_plan = reorder_stream_plan(
stream_plan
) # This may be redundant when using reorder_combined_plan
num_optimistic = sum(r.optimistic
for r in stream_plan) if stream_plan else 0
print('Stream plan ({}, {}, {:.3f}): {}\nAction plan ({}, {:.3f}): {}'.
format(get_length(stream_plan), num_optimistic,
compute_plan_effort(stream_plan), stream_plan,
get_length(action_plan), cost,
str_from_plan(action_plan)))
if is_plan(stream_plan) and visualize:
log_plans(stream_plan, action_plan, num_iterations)
create_visualizations(evaluations, stream_plan, num_iterations)
search_time += elapsed_time(start_time)
if (stream_plan is INFEASIBLE) and (not eager_instantiator) and (
not skeleton_queue) and (not disabled):
break
start_time = time.time()
if not is_plan(stream_plan):
complexity_limit += complexity_step
if not eager_disabled:
reenable_disabled(evaluations, disabled)
#print(stream_plan_complexity(evaluations, stream_plan))
if use_skeletons:
#optimizer_plan = replan_with_optimizers(evaluations, stream_plan, domain, optimizers)
optimizer_plan = None
if optimizer_plan is not None:
# TODO: post process a bound plan
print('Optimizer plan ({}, {:.3f}): {}'.format(
get_length(optimizer_plan),
compute_plan_effort(optimizer_plan), optimizer_plan))
skeleton_queue.new_skeleton(optimizer_plan, action_plan, cost)
allocated_sample_time = (search_sample_ratio * search_time) - sample_time \
if len(skeleton_queue.skeletons) <= max_skeletons else INF
skeleton_queue.process(stream_plan, action_plan, cost,
complexity_limit, allocated_sample_time)
else:
process_stream_plan(store,
domain,
disabled,
stream_plan,
action_plan,
cost,
bind=bind,
max_failures=max_failures)
sample_time += elapsed_time(start_time)
write_stream_statistics(externals, verbose)
return store.extract_solution()
