def create_dataset(self,
include_invalid=False,
include_none=True,
**kwargs):
# if (result.get(PARAMETER, None) is not None)
return Dataset(self, [
result for result in self.results
if implies(not result.get('valid', True), include_invalid)
and implies(result.get(SCORE, None) is None, include_none)
], **kwargs)
def pddlstream_from_tamp(tamp_problem, use_stream=True, use_optimizer=False, collisions=True):
initial = tamp_problem.initial
assert(initial.holding is None)
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
external_paths = []
if use_stream:
external_paths.append(get_file_path(__file__, 'stream.pddl'))
if use_optimizer:
external_paths.append(get_file_path(__file__, 'optimizer.pddl'))
external_pddl = [read(path) for path in external_paths]
constant_map = {}
init = [
('CanMove',),
('Conf', initial.conf),
('AtConf', initial.conf),
('HandEmpty',),
Equal((TOTAL_COST,), 0)] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', b, p) for b, p in initial.block_poses.items()] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] + \
[('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()] + \
[('Region', r) for r in tamp_problem.goal_regions.values() + [GROUND_NAME]]
goal_literals = [('In', b, r) for b, r in tamp_problem.goal_regions.items()] #+ [('HandEmpty',)]
if tamp_problem.goal_conf is not None:
goal_literals += [('AtConf', tamp_problem.goal_conf)]
goal = And(*goal_literals)
stream_map = {
's-motion': from_fn(plan_motion),
's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)),
't-region': from_test(get_region_test(tamp_problem.regions)),
's-ik': from_fn(inverse_kin_fn),
#'s-ik': from_gen_fn(unreliable_ik_fn),
'distance': distance_fn,
't-cfree': from_test(lambda *args: implies(collisions, not collision_test(*args))),
#'posecollision': collision_test, # Redundant
'trajcollision': lambda *args: False,
}
if use_optimizer:
stream_map.update({
'gurobi': from_fn(get_optimize_fn(tamp_problem.regions)),
'rrt': from_fn(cfree_motion_fn),
})
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map, init, goal)
def pddlstream_from_tamp(tamp_problem,
use_stream=True,
use_optimizer=False,
collisions=True):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
external_paths = []
if use_stream:
external_paths.append(get_file_path(__file__, 'stream.pddl'))
if use_optimizer:
external_paths.append(
get_file_path(
__file__,
'optimizer/optimizer.pddl')) # optimizer | optimizer_hard
external_pddl = [read(path) for path in external_paths]
constant_map = {}
stream_map = {
's-grasp':
from_fn(lambda b: (GRASP, )),
's-region':
from_gen_fn(get_pose_gen(tamp_problem.regions)),
's-ik':
from_fn(inverse_kin_fn),
#'s-ik': from_gen_fn(unreliable_ik_fn),
's-motion':
from_fn(plan_motion),
't-region':
from_test(get_region_test(tamp_problem.regions)),
't-cfree':
from_test(
lambda *args: implies(collisions, not collision_test(*args))),
'dist':
distance_fn,
'duration':
duration_fn,
}
if use_optimizer:
# To avoid loading gurobi
stream_map.update({
'gurobi':
from_list_fn(
get_optimize_fn(tamp_problem.regions, collisions=collisions)),
'rrt':
from_fn(cfree_motion_fn),
})
#stream_map = 'debug'
init, goal = create_problem(tamp_problem)
return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map,
init, goal)
def main():
# collect data in parallel, parameters are generated uniformly randomly in a range
# data stored to pour_date/trails_n=10000.json
# TODO: ulimit settings
# https://ss64.com/bash/ulimit.html
# https://stackoverflow.com/questions/938733/total-memory-used-by-python-process
# import psutil
# TODO: resource.getrusage(resource.RUSAGE_SELF).ru_maxrss
assert (get_python_version() == 3)
parser = argparse.ArgumentParser()
parser.add_argument('-f',
'--fn',
default=TRAINING,
help='The parameter function to use.')
parser.add_argument('-n',
'--num',
type=int,
default=10000,
help='The number of samples to collect.')
parser.add_argument('-p',
'--problem',
required=True,
choices=sorted(SKILL_COLLECTORS.keys()),
help='The name of the skill to learn.')
parser.add_argument('-t',
'--time',
type=int,
default=1 * 60,
help='The max planning runtime for each trial.')
parser.add_argument('-v',
'--visualize',
action='store_true',
help='When enabled, visualizes execution.')
args = parser.parse_args()
serial = is_darwin()
assert implies(args.visualize, serial)
trials = get_trials(args.problem,
args.fn,
args.num,
max_time=args.time,
valid=True,
visualize=args.visualize,
verbose=serial)
data_path = None if serial else get_data_path(args.problem, trials)
num_cores = get_num_cores(trials, serial)
user_input('Begin?')
# TODO: store the generating distribution for samples and objects?
print(SEPARATOR)
results = run_trials(trials, data_path, num_cores=num_cores)
def optimistic_stream_instantiation(instance, bindings, evaluations, opt_evaluations,
only_immediate=False):
# TODO: combination for domain predicates
new_instances = []
for input_combo in product(*[bindings.get(i, [i]) for i in instance.input_objects]):
mapping = get_mapping(instance.input_objects, input_combo)
domain_evaluations = set(map(evaluation_from_fact, substitute_expression(
instance.get_domain(), mapping))) # TODO: could just instantiate first
if domain_evaluations <= opt_evaluations:
new_instance = instance.external.get_instance(input_combo)
# TODO: method for eagerly evaluating some of these?
if (new_instance.opt_index != 0) and implies(only_immediate, domain_evaluations <= evaluations):
new_instance.opt_index -= 1
new_instances.append(new_instance)
return new_instances
def separate_plan(combined_plan, action_info=None, terminate=False, stream_only=True):
if not is_plan(combined_plan):
return combined_plan, combined_plan
stream_plan = []
action_plan = []
terminated = False
for operator in combined_plan:
if terminate and terminated:
break
if isinstance(operator, Result):
if terminated:
if implies(stream_only, isinstance(operator, StreamResult)):
action_plan.append(operator.get_tuple())
else:
stream_plan.append(operator)
else:
action_plan.append(operator)
if action_info is not None:
name = operator[0]
terminated |= action_info[name].terminal
return stream_plan, action_plan
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 main():
"""
./home/demo/catkin_percep/collect_scales.sh (includes scale offset)
Make sure to start the scales with nothing on them (for calibration)
"""
assert (get_python_version() == 2) # ariadne has ROS with python2
parser = argparse.ArgumentParser()
parser.add_argument('-a',
'--active',
action='store_true',
help='Uses active learning queries.')
parser.add_argument('-d',
'--debug',
action='store_true',
help='Disables saving during debugging.')
parser.add_argument(
'-f',
'--fn',
type=str,
default=TRAINING, # DESIGNED | TRAINING
help='The name of or the path to the policy that generates parameters.'
)
parser.add_argument('-m',
'--material',
required=True,
choices=sorted(MATERIALS),
help='The name of the material being used.')
parser.add_argument('-p',
'--problem',
required=True,
choices=sorted(REQUIREMENT_FNS.keys()),
help='The name of the skill to learn.')
parser.add_argument('-s',
'--spoon',
default=None,
choices=SPOONS,
help='The name of the spoon being used.')
parser.add_argument('-r',
'--train',
action='store_true',
help='When enabled, uses the training dataset.')
parser.add_argument(
'-v',
'--visualize_planning',
action='store_true',
help=
'When enabled, visualizes planning rather than the world (for debugging).'
)
args = parser.parse_args()
# TODO: toggle material default based on task
# TODO: label material on the image
assert args.material in MODEL_MASSES
print('Policy:', args.fn)
assert implies(args.problem in ['scoop'], args.spoon is not None)
assert implies(args.active, args.train)
ros_world = ROSWorld(sim_only=False, visualize=not args.visualize_planning)
classes_pub = rospy.Publisher('~collect_classes', String, queue_size=1)
with ros_world:
set_camera_pose(np.array([1.5, -0.5, 1.5]),
target_point=np.array([0.75, 0, 0.75]))
arm, is_open = ACTIVE_ARMS[args.problem]
open_grippers = {arm: is_open}
if args.problem == 'scoop':
ros_world.controller.open_gripper(get_arm_prefix(arm), blocking=True)
ros_world.controller.speak("{:.0f} seconds to attach {}".format(
ATTACH_TIME, format_class(args.spoon)))
rospy.sleep(ATTACH_TIME) # Sleep to have time to set the spoon
move_to_initial_config(ros_world, open_grippers) #get_other_arm
# TODO: cross validation for measuring performance across a few bowls
launch = launch_kinect()
video_time = time.time()
if args.debug:
ros_world.controller.speak("Warning! Data will not be saved.")
time.sleep(1.0)
data_path = None
else:
# TODO: only create directory if examples made
data_path = get_data_path(args.problem, real=True)
# TODO: log camera image after the pour
policy = args.fn
learner_path = None
test_data = None
if isinstance(policy, str) and os.path.isfile(policy):
policy = read_pickle(policy)
assert isinstance(policy, ActiveLearner)
print(policy)
print(policy.algorithm)
#policy.transfer_weight = 0
# print(policy.xx.shape)
# policy.results = policy.results[:-1]
# policy.xx = policy.xx[:-1]
# policy.yy = policy.yy[:-1]
# policy.weights = policy.weights[:-1]
# print(policy.xx.shape)
# write_pickle(args.fn, policy)
# print('Saved', args.fn)
if args.active:
# policy.retrain()
test_domain = load_data(SCOOP_TEST_DATASETS, verbose=False)
test_data = test_domain.create_dataset(include_none=False,
binary=False)
policy.query_type = STRADDLE # VARIANCE
#policy.weights = 0.1*np.ones(policy.yy.shape) # TODO: make this multiplicative
#policy.retrain()
evaluate_confusions(test_data, policy)
else:
policy.query_type = BEST
ensure_dir(LEARNER_DIRECTORY)
date_name = datetime.datetime.now().strftime(DATE_FORMAT)
filename = '{}_{}.pk{}'.format(get_label(policy.algorithm), date_name,
get_python_version())
learner_path = os.path.join(LEARNER_DIRECTORY, filename)
if ACTIVE_FEATURE and args.active:
assert isinstance(policy, ActiveLearner)
generator = create_active_generator(args, policy)
else:
generator = create_random_generator(args)
pair = next(generator)
print('Next pair:', pair)
classes_pub.publish('{},{}'.format(*pair))
for phrase in map(format_class, pair):
ros_world.controller.speak(phrase)
wait_for_user('Press enter to begin')
# TODO: change the name of the directory after additional samples
results = []
num_trials = num_failures = num_scored = 0
while True:
start_time = elapsed_time(video_time)
result = run_loop(args, ros_world, policy)
print('Result:', str_from_object(result))
print('{}\nTrials: {} | Successes: {} | Failures: {} | Time: {:.3f}'.
format(SEPARATOR, num_trials, len(results), num_failures,
elapsed_time(video_time)))
num_trials += 1
if result is None: # TODO: result['execution']
num_failures += 1
print('Error! Trial resulted in an exception')
move_to_initial_config(ros_world, open_grippers)
continue
end_time = elapsed_time(video_time)
print('Elapsed time:', end_time - start_time)
# TODO: record the type of failure (planning, execution, etc...)
scored = result['score'] is not None
num_scored += scored
# TODO: print the score
if isinstance(policy, ActiveLearner) and args.active: # and scored:
update_learner(policy, learner_path, result)
evaluate_confusions(test_data, policy)
# TODO: how to handle failures that require bad annotations?
pair = next(generator)
print('Next pair:', pair)
classes_pub.publish('{},{}'.format(*pair))
for phrase in map(format_class, pair):
ros_world.controller.speak(phrase)
annotation = wait_for_user(
'Enter annotation and press enter to continue: ')
result.update({
# TODO: record the query_type
'policy': args.fn,
'active_feature': ACTIVE_FEATURE,
'trial': num_trials,
'start_time': start_time,
'end_time': end_time,
'annotation': annotation,
})
results.append(result)
if data_path is not None:
write_results(data_path, results)
#if annotation in ['q', 'quit']: # TODO: Ctrl-C to quit
# break
ros_world.controller.speak("Finished")
if launch is not None:
launch.shutdown()
print('Total time:', elapsed_time(video_time))
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 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('experiments',
nargs='+',
help='Name of the experiment')
args = parser.parse_args()
outcomes_per_task = {}
for path in args.experiments:
for result in read_json(path):
experiment = result['experiment']
problem = experiment['problem']
outcome = result['outcome']
#policy = frozenset(experiment['policy'].items())
policy = name_from_policy(experiment['policy'])
outcomes_per_task.setdefault(problem['task'],
{}).setdefault(policy,
[]).append(outcome)
#outcomes_per_task['inspect_drawer']['constrain=0_defer=1'].append(ERROR_OUTCOME)
#outcomes_per_task['detect_block']['constrain=1_defer=0'].append(ERROR_OUTCOME)
# TODO: robust poses
# TODO: intelligent IR for pour
table = ''
for task in TASK_NAMES:
if task not in outcomes_per_task:
continue
print('\nTask: {}'.format(task))
items = [task]
for policy in POLICIES:
policy = name_from_policy(policy)
if policy not in outcomes_per_task[task]:
continue
outcomes = list(take(outcomes_per_task[task][policy], MAX_TRIALS))
value_per_attribute = {}
for outcome in outcomes:
if outcome['error']:
outcome.update(ERROR_OUTCOME)
if MAX_TIME < outcome.get('total_time', INF):
outcome['achieved_goal'] = False
if not outcome['achieved_goal']:
outcome['total_time'] = MAX_TIME
outcome['plan_time'] = MAX_TIME
for attribute, value in outcome.items():
if (attribute not in ['policy']) and (attribute in PRINT_ATTRIBUTES) and \
not isinstance(value, str) and implies(attribute in ACHIEVED_GOAL,
outcome['achieved_goal']):
value_per_attribute.setdefault(attribute,
[]).append(float(value))
statistics = {
attribute: np.round(np.average(values), 3) # '{:.2f}'.format(
for attribute, values in value_per_attribute.items()
} # median, min, max of solved?
statistics['trials'] = len(outcomes)
print('{}: {}'.format(policy, str_from_object(statistics)))
items += [
'{:.0f}'.format(100 * statistics['achieved_goal']),
'{:.0f}'.format(statistics['plan_time']),
]
table += '{}\n\\\\ \hline\n'.format(' & '.join(items))
print(SEPARATOR)
print(POLICIES)
print(table)
def process_stream_plan(evaluations,
stream_plan,
disabled,
verbose,
quick_fail=True,
layers=False,
max_values=INF):
# TODO: can also use the instantiator and operate directly on the outputs
# TODO: could bind by just using new_evaluations
plan_index = get_stream_plan_index(stream_plan)
streams_from_output = defaultdict(list)
for result in stream_plan:
if isinstance(result, StreamResult):
for obj in result.output_objects:
streams_from_output[obj].append(result)
shared_output_streams = {
s
for streams in streams_from_output.values() if 1 < len(streams)
for s in streams
}
#shared_output_streams = {}
print(shared_output_streams)
print(plan_index)
opt_bindings = defaultdict(list)
opt_evaluations = set()
opt_results = []
failed = False
stream_queue = deque(stream_plan)
while stream_queue and implies(quick_fail, not failed):
opt_result = stream_queue.popleft()
real_instances, opt_instances = ground_stream_instances(
opt_result.instance, opt_bindings, evaluations, opt_evaluations,
plan_index)
first_step = all(
isinstance(o, Object) for o in opt_result.instance.input_objects)
num_instances = min(len(real_instances), max_values) \
if (layers or first_step or (opt_result not in shared_output_streams)) else 0
opt_instances += real_instances[num_instances:]
real_instances = real_instances[:num_instances]
new_results = []
local_failure = False
for instance in real_instances:
results = instance.next_results(verbose=verbose)
for result in results:
add_certified(evaluations, result)
disable_stream_instance(instance, disabled)
local_failure |= not results
if isinstance(opt_result, PredicateResult) and not any(
opt_result.value == r.value for r in results):
local_failure = True # TODO: check for instance?
new_results += results
for instance in opt_instances:
#print(instance, instance.opt_index)
results = instance.next_optimistic()
opt_evaluations.update(
evaluation_from_fact(f) for r in results
for f in r.get_certified())
opt_results += results
local_failure |= not results
new_results += results
for result in new_results:
if isinstance(result, StreamResult): # Could not add if same value
for opt, obj in zip(opt_result.output_objects,
result.output_objects):
opt_bindings[opt].append(obj)
if local_failure and isinstance(opt_result, SynthStreamResult):
stream_queue.extendleft(reversed(opt_result.decompose()))
failed = False # TODO: check if satisfies target certified
else:
failed |= local_failure
if verbose:
print('Success: {}'.format(not failed))
if failed:
return None, None
# TODO: just return binding
# TODO: could also immediately switch to binding if plan_index == 0 afterwards
return opt_results, opt_bindings
def pddlstream_from_tamp(tamp_problem,
use_stream=True,
use_optimizer=False,
collisions=True):
initial = tamp_problem.initial
assert (not initial.holding)
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
external_paths = []
if use_stream:
external_paths.append(get_file_path(__file__, 'stream.pddl'))
if use_optimizer:
external_paths.append(get_file_path(
__file__, 'optimizer.pddl')) # optimizer | optimizer_hard
external_pddl = [read(path) for path in external_paths]
constant_map = {}
init = [
('Region', GROUND_NAME),
Equal((TOTAL_COST,), 0)] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', b, p) for b, p in initial.block_poses.items()] + \
[('Grasp', b, GRASP) for b in initial.block_poses] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()]
goal_literals = []
for r, q in initial.robot_confs.items():
init += [
('Robot', r),
('CanMove', r),
('Conf', q),
('AtConf', r, q),
('HandEmpty', r),
]
if tamp_problem.goal_conf is not None:
#goal_literals += [('AtConf', tamp_problem.goal_conf)]
goal_literals += [('AtConf', r, q)]
for b, r in tamp_problem.goal_regions.items():
if isinstance(r, str):
init += [('Region', r), ('Placeable', b, r)]
goal_literals += [('In', b, r)]
else:
init += [('Pose', b, r)]
goal_literals += [('AtPose', b, r)]
#goal_literals += [Not(('Unsafe',))] # ('HandEmpty',)
goal = And(*goal_literals)
stream_map = {
's-motion':
from_fn(plan_motion),
's-region':
from_gen_fn(get_pose_gen(tamp_problem.regions)),
't-region':
from_test(get_region_test(tamp_problem.regions)),
's-ik':
from_fn(inverse_kin_fn),
#'s-ik': from_gen_fn(unreliable_ik_fn),
'dist':
distance_fn,
'duration':
duration_fn,
't-cfree':
from_test(
lambda *args: implies(collisions, not collision_test(*args))),
}
if use_optimizer:
# To avoid loading gurobi
stream_map.update({
'gurobi':
from_list_fn(
get_optimize_fn(tamp_problem.regions, collisions=collisions)),
'rrt':
from_fn(cfree_motion_fn),
})
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map,
init, goal)
def main():
parser = argparse.ArgumentParser()
parser.add_argument('paths', nargs='*', help='Paths to the data.')
#parser.add_argument('-a', '--active', type=int, default=0, # None
# help='The number of active samples to collect')
parser.add_argument(
'-d',
'--deterministic',
action='store_true',
help='Whether to deterministically create training splits')
parser.add_argument('-n',
'--num_trials',
type=int,
default=-1,
help='The number of samples to collect')
parser.add_argument('-s',
'--save',
action='store_true',
help='Whether to save the learners')
parser.add_argument('-r',
'--num_rounds',
type=int,
default=1,
help='The number of rounds to collect')
parser.add_argument('-t',
'--test',
action='store_true',
help='Whether to save the data')
parser.add_argument('-v',
'--visualize',
action='store_true',
help='When enabled, visualizes execution.')
args = parser.parse_args()
# TODO: be careful that paging isn't altering the data
# TODO: use a different set of randomized parameters for train and test
serial = is_darwin()
visualize = serial and args.visualize
assert implies(visualize, serial)
num_trials = get_max_cores(
serial) if args.num_trials < 0 else args.num_trials
##################################################
#train_sizes = inclusive_range(50, 200, 10) # Best
#train_sizes = inclusive_range(50, 400, 10) # F1
#train_sizes = inclusive_range(25, 400, 25)
#train_sizes = inclusive_range(50, 100, 5) # Real
#train_sizes = inclusive_range(100, 200, 5)
#train_sizes = inclusive_range(10, 250, 5)
#train_sizes = inclusive_range(35, 70, 5)
#train_sizes = inclusive_range(5, 50, 5)
#train_sizes = inclusive_range(40, 80, 5)
#train_sizes = inclusive_range(100, 1000, 100)
#train_sizes = [50]
#train_sizes = [250]
train_sizes = [1000]
#train_sizes = [327] # train + test
#train_sizes = inclusive_range(5, 150, 25)
#train_sizes = [100]
#kernels = ['RBF', 'Matern52', 'MLP']
kernels = ['MLP']
hyperparams = [None]
#hyperparams = [True]
#hyperparams = [None, True]
query_type = BEST # BEST | CONFIDENT | REJECTION | ACTIVE # type of query used to evaluate the learner
include_none = False
binary = False
# 0 => no transfer
# 1 => mean transfer
# 2 => kernel transfer
# 3 => both transfer
transfer_weights = [None]
#transfer_weights = list(range(4))
#transfer_weights = [0, 1]
#transfer_weights = [3]
#transfer_weights = np.around(np.linspace(0.0, 1.0, num=1+5, endpoint=True), decimals=3) # max 10 colors
#transfer_weights = list(range(1, 1+3))
#split = UNIFORM # BALANCED
#print('Split:', split)
#parameters = {
# 'include None': include_none,
# 'binary': binary,
# 'split': split,
#}
# Omitting failed labels is okay because they will never be executed
algorithms = []
#algorithms += [(Algorithm(nn_model, label='NN'), [num])
# for nn_model, num in product(NN_MODELS, train_sizes)]
#algorithms += [(Algorithm(RANDOM), None), (Algorithm(DESIGNED), None)]
#algorithms += [(Algorithm(RF_CLASSIFIER, variance=False, transfer_weight=tw, label='RF'), [num])
# for num, tw in product(train_sizes, [None])] # transfer_weights
#algorithms += [(Algorithm(RF_REGRESSOR, variance=False, transfer_weight=tw, label='RF'), [num])
# for num, tw in product(train_sizes, [None])] # transfer_weights
#algorithms += [(Algorithm(BATCH_RF, variance=True, transfer_weight=tw, label='RF'), [num])
# for num, tw in product(train_sizes, [None])] # transfer_weights
#algorithms += [(Algorithm(BATCH_MAXVAR_RF, variance=True, transfer_weight=tw), train_sizes)
# for tw in product(use_vars, [None])] # transfer_weights
#algorithms += [(Algorithm(BATCH_STRADDLE_RF, variance=True, transfer_weight=tw), train_sizes)
# for tw, in product([None])] # transfer_weights
use_vars = [True]
# STRADDLE is better than MAXVAR when the learner has a good estimate of uncertainty
algorithms += [
(Algorithm(BATCH_GP, kernel, hype, use_var, tw,
label='GP'), [num]) # label='GP-{}'.format(kernel)
for num, kernel, hype, use_var, tw in product(
train_sizes, kernels, hyperparams, use_vars, transfer_weights)
]
#algorithms += [(Algorithm(BATCH_MAXVAR_GP, kernel, hype, True, tw, label='GP-Var'), train_sizes)
# for kernel, hype, tw in product(kernels, hyperparams, transfer_weights)]
#algorithms += [(Algorithm(BATCH_STRADDLE_GP, kernel, hype, True, tw, label='GP-LSE'), train_sizes)
# for kernel, hype, tw in product(kernels, hyperparams, transfer_weights)] # default active
#algorithms += [(Algorithm(BATCH_STRADDLE_GP, kernel, hype, True, tw, label='GP-LSE2'), train_sizes)
# for kernel, hype, tw in product(kernels, hyperparams, transfer_weights)] # active control only
# algorithms += [(Algorithm(MAXVAR_GP, kernel, hype, use_var), train_sizes)
# for kernel, hype, use_var in product(kernels, hyperparams, use_vars)]
#algorithms += [(Algorithm(STRADDLE_GP, kernel, hype, use_var, tw), train_sizes)
# for kernel, hype, use_var, tw in product(kernels, hyperparams, use_vars, transfer_weights)]
#batch_sizes = inclusive_range(train_sizes[0], 90, 10)
#step_size = 10 # TODO: extract from train_sizes
#final_size = train_sizes[-1]
# Previously didn't have use_var=True
# algorithms += [(Algorithm(BATCH_STRADDLE_GP, kernel, hyperparameters=batch_size, variance=True, transfer_weight=tw),
# inclusive_range(batch_size, final_size, step_size))
# for kernel, tw, batch_size in product(kernels, transfer_weights, batch_sizes)]
# algorithms += [(Algorithm(BATCH_STRADDLE_RF, hyperparameters=batch_size, variance=True, transfer_weight=tw),
# inclusive_range(batch_size, final_size, step_size))
# for tw, batch_size in product(transfer_weights, batch_sizes)]
print('Algorithms:', algorithms)
##################################################
real_world = not args.paths
transfer_domain = load_data(TRANSFER_DATASETS, verbose=False)
transfer_algorithm = None
if real_world and transfer_weights != [None]:
#assert transfer_weights[0] is not None
transfer_data = transfer_domain.create_dataset(
include_none=include_none, binary=binary)
transfer_algorithm = Algorithm(BATCH_GP,
kernel=kernels[0],
variance=use_vars[0])
validity_learner = None
#validity_learner = create_validity_classifier(transfer_domain)
##################################################
train_paths = args.paths
if real_world:
train_paths = SCOOP_TRAIN_DATASETS # TRAIN_DATASETS
#train_paths = TRANSFER_DATASETS
#train_paths = TRAIN_DATASETS + TRANSFER_DATASETS # Train before transfer
#scale_paths = TRAIN_DATASETS + TEST_DATASETS
scale_paths = None
print(SEPARATOR)
print('Train paths:', train_paths)
domain = load_data(train_paths)
print()
print(domain)
all_data = domain.create_dataset(include_none=include_none,
binary=binary,
scale_paths=scale_paths)
#all_data.results = all_data.results[:1000]
num_failed = 0
#num_failed = 100
failed_domain = transfer_domain if real_world else domain
failed_results = randomize(
result for result in failed_domain.results
if not result.get('success', False))[:num_failed]
#failed_data = Dataset(domain, failed_results, **all_data.kwargs)
test_paths = SCOOP_TEST_DATASETS # TEST_DATASETS | SCOOP_TEST_DATASETS
#test_paths = None
if real_world and not (set(train_paths) & set(test_paths)):
#assert not set(train_paths) & set(test_paths)
#max_test = 0
test_data = load_data(test_paths).create_dataset(
include_none=False, binary=binary, scale_paths=scale_paths)
else:
#assert scale_paths is None # TODO: max_train will be too small otherwise
test_paths = test_data = None
print(SEPARATOR)
print('Test paths:', test_paths)
all_active_data = None
#if real_world:
# all_active_data = load_data(ACTIVE_DATASETS).create_dataset(include_none=True, binary=binary, scale_paths=scale_paths)
# TODO: could include OS and username if desired
date_name = datetime.datetime.now().strftime(DATE_FORMAT)
size_str = '[{},{}]'.format(train_sizes[0], train_sizes[-1])
#size_str = '-'.join(map(str, train_sizes))
experiments_name = '{}_r={}_t={}_n={}'.format(date_name, args.num_rounds,
size_str, num_trials)
trials_per_round = sum(
1 if train_sizes is None else (train_sizes[-1] - train_sizes[0] +
len(train_sizes))
for _, train_sizes in algorithms)
num_experiments = args.num_rounds * trials_per_round
max_train = min(
max([0] + [
active_sizes[0]
for _, active_sizes in algorithms if active_sizes is not None
]), len(all_data))
max_test = min(len(all_data) - max_train, 1000)
##################################################
# #features = ['bowl_height']
# features = ['spoon_height']
# #features = ['bowl_height', 'spoon_height']
# X, Y, _ = all_data.get_data()
# #indices = [domain.inputs.index(feature) for feature in features]
# #X = X[:,indices]
# X = [[result[FEATURE][name] for name in features] for result in all_data.results]
# from sklearn.linear_model import LinearRegression
# model = LinearRegression(fit_intercept=True, normalize=False)
# model.fit(X, Y)
# #print(model.get_params())
# print(model.coef_.tolist(), model.intercept_)
# print(model.score(X, Y))
#data_dir = os.path.join(DATA_DIRECTORY, domain.name) # EXPERIMENT_DIRECTORY
data_dir = os.path.abspath(os.path.join(domain.name, os.path.pardir))
experiments_dir, data_path = None, None
if not args.test or not serial:
experiments_dir = os.path.join(data_dir, experiments_name)
data_path = os.path.join(
experiments_dir, 'experiments.pk{}'.format(get_python_version()))
##################################################
print(SEPARATOR)
print('Name:', experiments_name)
print('Experiments:', num_experiments)
print('Experiment dir:', experiments_dir)
print('Data path:', data_path)
print('Examples:', len(all_data))
print('Valid:',
sum(result.get('valid', True) for result in all_data.results))
print('Success:',
sum(result.get('success', False) for result in all_data.results))
print(
'Scored:',
sum(
result.get('score', None) is not None
for result in all_data.results))
print('Max train:', max_train)
print('Max test:', max_test)
print('Include None:', include_none)
print('Examples: n={}, d={}'.format(len(all_data), domain.dx))
print('Binary:', binary)
print('Serial:', serial)
print('Estimated hours: {:.3f}'.format(num_experiments *
SEC_PER_EXPERIMENT / HOURS_TO_SECS))
user_input('Begin?')
##################################################
experiments = []
if experiments_dir is not None:
mkdir(experiments_dir)
# if os.path.exists(data_path):
# experiments.extend(read_pickle(data_path))
# TODO: embed in a KeyboardInterrupt to allow early termination
start_time = time.time()
for round_idx in range(args.num_rounds):
seed = round_idx if args.deterministic else hash(
time.time()) # vs just time.time()?
random.seed(seed)
all_data.shuffle()
if test_paths is None: # cannot use test_data
#test_data, train_data = split_data(all_data, max_test)
train_data = test_data = all_data # Training performance
else:
train_data = all_data
transfer_learner = None
if transfer_algorithm is not None:
round_data, _ = transfer_data.partition(index=1000)
transfer_learner, _ = create_learner(transfer_domain,
round_data,
transfer_algorithm,
verbose=True)
transfer_learner.retrain()
print(SEPARATOR)
print('Round {} | Train examples: {} | Test examples: {}'.format(
round_idx, len(train_data), len(test_data)))
for algorithm, active_sizes in algorithms:
# active_sizes = [first #trainingdata selected from X_train, #active exploration + #trainingdata]
print(SEPARATOR)
print('Round: {} | {} | Seed: {} | Sizes: {}'.format(
round_idx, algorithm, seed, active_sizes))
# TODO: allow keyboard interrupt
if active_sizes is None:
learner = algorithm.name
active_size = train_confusion = None
experiments.append(
evaluate_learner(domain, seed, train_confusion, test_data,
algorithm, learner, active_size,
num_trials, serial, args.visualize))
continue
# [10 20 25] take first 10 samples from X_train to train the model, 10 samples chosen actively
# sequentially + evaluate model, 5 samples chosen actively sequentially + evaluate model
# Could always keep around all the examples and retrain
# TODO: segfaults when this runs in parallel
# TODO: may be able to retrain in parallel if I set OPENBLAS_NUM_THREADS
num_batch = active_sizes[0]
batch_data, active_data = train_data.partition(num_batch)
if all_active_data is not None:
active_data = all_active_data.clone()
#batch_data.results.extend(failed_results)
learner, train_confusion = create_learner(
domain,
batch_data,
algorithm, # alphas,
query_type=query_type,
verbose=True)
learner.validity_learner = validity_learner
if transfer_learner is not None:
learner.sim_model = transfer_learner.model
learner.retrain()
for active_size in active_sizes:
num_active = active_size - (learner.nx - len(failed_results))
print('\nRound: {} | {} | Seed: {} | Size: {} | Active: {}'.
format(round_idx, algorithm, seed, active_size,
num_active))
if algorithm.name in CONTINUOUS_ACTIVE_GP:
active_learning(learner, num_active, visualize=visualize)
#active_learning(learner, num_active, discrete_feature=True, random_feature=False)
#active_learning_discrete(learner, active_data, num_active, random_feature=False)
elif algorithm.name in BATCH_ACTIVE:
active_learning_discrete(learner, active_data, num_active)
#active_learning(learner, num_active, discrete_feature=True, random_feature=True)
#active_learning_discrete(learner, active_data, num_active, random_feature=True)
#if round_dir is not None:
# save_learner(round_dir, learner)
if args.save:
learner.save(data_dir)
experiments.append(
evaluate_learner(domain, seed, train_confusion, test_data,
algorithm, learner, active_size,
num_trials, serial, args.visualize))
save_experiments(data_path, experiments)
print(SEPARATOR)
if experiments:
save_experiments(data_path, experiments)
plot_experiments(domain,
experiments_name,
experiments_dir,
experiments,
include_none=False)
print('Experiments: {}'.format(experiments_dir))
print('Total experiments: {}'.format(len(experiments)))
print('Total hours: {:.3f}'.format(
elapsed_time(start_time) / HOURS_TO_SECS))
def pdddlstream_from_problem(belief,
additional_init=[],
fixed_base=True,
**kwargs):
world = belief.world # One world per state
task = world.task # One task per world
print(task)
domain_pddl = read(get_file_path(__file__, '../pddl/domain.pddl'))
# TODO: repackage stream outputs to avoid recomputation
# Despite the base not moving, it could be re-estimated
init_bq = belief.base_conf
init_aq = belief.arm_conf
init_gq = belief.gripper_conf
carry_aq = world.carry_conf
init_aq = carry_aq if are_confs_close(init_aq, carry_aq) else init_aq
# TODO: the following doesn't work. Maybe because carry_conf is used elsewhere
#carry_aq = init_aq if are_confs_close(init_aq, world.carry_conf) else world.carry_conf
#calibrate_aq = init_aq if are_confs_close(init_aq, world.calibrate_conf) else world.calibrate_conf
# Don't need this now that returning to old confs
#open_gq = init_gq if are_confs_close(init_gq, world.open_gq) else world.open_gq
#closed_gq = init_gq if are_confs_close(init_gq, world.closed_gq) else world.closed_gq
open_gq = world.open_gq
closed_gq = world.closed_gq
constant_map = {
'@world': 'world',
'@gripper': 'gripper',
'@stove': 'stove',
'@none': None,
'@rest_aq': carry_aq,
#'@calibrate_aq': calibrate_aq,
'@open_gq': open_gq,
'@closed_gq': closed_gq,
'@open': OPEN,
'@closed': CLOSED,
'@top': TOP_GRASP,
'@side': SIDE_GRASP,
'@bq0': init_bq,
}
top_joint = JOINT_TEMPLATE.format(TOP_DRAWER)
bottom_joint = JOINT_TEMPLATE.format(BOTTOM_DRAWER)
init = [
('BConf', init_bq),
('AtBConf', init_bq),
('AConf', init_bq, carry_aq),
#('RestAConf', carry_aq),
#('AConf', init_bq, calibrate_aq),
(
'Stationary', ),
('AConf', init_bq, init_aq),
('AtAConf', init_aq),
('GConf', open_gq),
('GConf', closed_gq),
('Grasp', None, None),
('AtGrasp', None, None),
('Above', top_joint, bottom_joint),
('Adjacent', top_joint, bottom_joint),
('Adjacent', bottom_joint, top_joint),
('Calibrated', ),
('CanMoveBase', ),
('CanMoveArm', ),
('CanMoveGripper', ),
] + list(task.init) + list(additional_init)
for action_name, cost in ACTION_COSTS.items():
function_name = '{}Cost'.format(title_from_snake(action_name))
function = (function_name, )
init.append(Equal(function, cost)) # TODO: stove state
init += [('Stackable', name, surface) for name, surface in task.goal_on.items()] + \
[('Stackable', name, stove) for name, stove in product(task.goal_cooked, STOVES)] + \
[('Pressed', name) for name in belief.pressed] + \
[('Cookable', name) for name in task.goal_cooked] + \
[('Cooked', name) for name in belief.cooked] + \
[('Status', status) for status in DOOR_STATUSES] + \
[('Knob', knob) for knob in KNOBS] + \
[('Joint', knob) for knob in KNOBS] + \
[('Liquid', liquid) for _, liquid in task.init_liquid] + \
[('HasLiquid', cup, liquid) for cup, liquid in belief.liquid] + \
[('StoveKnob', STOVE_TEMPLATE.format(loc), KNOB_TEMPLATE.format(loc)) for loc in STOVE_LOCATIONS] + \
[('GraspType', ty) for ty in task.grasp_types] # TODO: grasp_type per object
#[('Type', obj_name, 'stove') for obj_name in STOVES] + \
#[('Camera', name) for name in world.cameras]
if task.movable_base:
init.append(('MovableBase', ))
if fixed_base:
init.append(('InitBConf', init_bq))
if task.noisy_base:
init.append(('NoisyBase', ))
compute_pose_kin = get_compute_pose_kin(world)
compute_angle_kin = get_compute_angle_kin(world)
initial_poses = {}
for joint_name, init_conf in belief.door_confs.items():
if joint_name in DRAWER_JOINTS:
init.append(('Drawer', joint_name))
if joint_name in CABINET_JOINTS:
init.append(('Cabinet', joint_name))
joint = joint_from_name(world.kitchen, joint_name)
surface_name = surface_from_joint(joint_name)
init.append(('SurfaceJoint', surface_name, joint_name))
# Relies on the fact that drawers have identical surface and link names
link_name = get_link_name(world.kitchen, child_link_from_joint(joint))
#link_name = str(link_name.decode('UTF-8'))
#link_name = str(link_name.encode('ascii','ignore'))
for conf in {
init_conf, world.open_kitchen_confs[joint],
world.closed_kitchen_confs[joint]
}:
# TODO: return to initial poses?
world_pose, = compute_angle_kin(link_name, joint_name, conf)
init.extend([
('Joint', joint_name),
('Angle', joint_name, conf),
('Obstacle', link_name),
('AngleKin', link_name, world_pose, joint_name, conf),
('WorldPose', link_name, world_pose),
])
if joint in world.kitchen_joints:
init.extend([
('Sample', world_pose),
#('Value', world_pose), # comment out?
])
if conf == init_conf:
initial_poses[link_name] = world_pose
init.extend([
('AtAngle', joint_name, conf),
('AtWorldPose', link_name, world_pose),
])
for surface_name in ALL_SURFACES:
if surface_name in OPEN_SURFACES:
init.append(('Counter', surface_name)) # Fixed surface
if surface_name in DRAWERS:
init.append(('Drawer', surface_name))
surface = surface_from_name(surface_name)
surface_link = link_from_name(world.kitchen, surface.link)
parent_joint = parent_joint_from_link(surface_link)
if parent_joint not in world.kitchen_joints:
# TODO: attach to world frame?
world_pose = RelPose(world.kitchen, surface_link, init=True)
initial_poses[surface_name] = world_pose
init += [
#('RelPose', surface_name, world_pose, 'world'),
('WorldPose', surface_name, world_pose),
#('AtRelPose', surface_name, world_pose, 'world'),
('AtWorldPose', surface_name, world_pose),
('Sample', world_pose),
#('Value', world_pose),
]
init.extend([
('CheckNearby', surface_name),
#('InitPose', world_pose),
('Localized', surface_name),
])
for grasp_type in task.grasp_types:
if (surface_name in OPEN_SURFACES) or has_place_database(
world.robot_name, surface_name, grasp_type):
init.append(('AdmitsGraspType', surface_name, grasp_type))
if belief.grasped is None:
init.extend([
('HandEmpty', ),
('GConf', init_gq),
('AtGConf', init_gq),
])
else:
obj_name = belief.grasped.body_name
assert obj_name not in belief.pose_dists
grasp = belief.grasped
init += [
# Static
#('Graspable', obj_name),
('Grasp', obj_name, grasp),
('IsGraspType', obj_name, grasp, grasp.grasp_type),
# Fluent
('AtGrasp', obj_name, grasp),
('Holding', obj_name),
('Localized', obj_name),
]
init.extend(('ValidGraspType', obj_name, grasp_type)
for grasp_type in task.grasp_types
if implies(world.is_real(),
is_valid_grasp_type(obj_name, grasp_type)))
for obj_name in world.movable:
obj_type = type_from_name(obj_name)
if obj_type in BOWLS:
init.append(('Bowl', obj_name))
else:
init.append(
('Obstacle', obj_name)) # TODO: hack to place within bowls
if obj_type in COOKABLE:
init.append(('Cookable', obj_name))
if obj_type in POURABLE:
init.append(('Pourable', obj_name))
init += [
('Entity', obj_name),
('CheckNearby', obj_name),
] + [('Stackable', obj_name, counter)
for counter in set(ALL_SURFACES) & set(COUNTERS)]
# TODO: track poses over time to produce estimates
for obj_name, pose_dist in belief.pose_dists.items():
dist_support = pose_dist.dist.support()
localized = pose_dist.is_localized()
graspable = True
if localized:
init.append(('Localized', obj_name))
[rel_pose] = dist_support
roll, pitch, yaw = euler_from_quat(
quat_from_pose(rel_pose.get_reference_from_body()))
if (MAX_ERROR < abs(roll)) or (MAX_ERROR < abs(pitch)):
graspable = False
print(
'{} has an invalid orientation: roll={:.3f}, pitch={:.3f}'.
format(obj_name, roll, pitch))
if graspable:
#init.append(('Graspable', obj_name))
init.extend(('ValidGraspType', obj_name, grasp_type)
for grasp_type in task.grasp_types
if implies(world.is_real(),
is_valid_grasp_type(obj_name, grasp_type)))
# Could also fully decompose into points (but many samples)
# Could immediately add likely points for collision checking
for rel_pose in (dist_support if localized else pose_dist.decompose()):
surface_name = rel_pose.support
if surface_name is None:
# Treats as obstacle
# TODO: could temporarily add to fixed
world_pose = rel_pose
init += [
('WorldPose', obj_name, world_pose),
('AtWorldPose', obj_name, world_pose),
]
poses = [world_pose]
#raise RuntimeError(obj_name, supporting)
else:
surface_pose = initial_poses[surface_name]
world_pose, = compute_pose_kin(obj_name, rel_pose,
surface_name, surface_pose)
init += [
# Static
('RelPose', obj_name, rel_pose, surface_name),
('WorldPose', obj_name, world_pose),
('PoseKin', obj_name, world_pose, rel_pose, surface_name,
surface_pose),
# Fluent
('AtRelPose', obj_name, rel_pose, surface_name),
('AtWorldPose', obj_name, world_pose),
]
if localized:
init.append(('On', obj_name, surface_name))
poses = [rel_pose, world_pose]
for pose in poses:
if isinstance(pose, PoseDist):
init.append(('Dist', pose))
else:
init.extend([('Sample', pose)]) #, ('Value', pose)])
#for body, ty in problem.body_types:
# init += [('Type', body, ty)]
#bodies_from_type = get_bodies_from_type(problem)
#bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter(ty) else [ty]
goal_formula = get_goal(belief, init)
stream_pddl, stream_map = get_streams(world,
teleport_base=task.teleport_base,
**kwargs)
print('Constants:', constant_map)
print('Init:', sorted(init, key=lambda f: f[0]))
print('Goal:', goal_formula)
#print('Streams:', stream_map.keys()) # DEBUG
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal_formula)
def solve_focused(problem,
constraints=PlanConstraints(),
stream_info={},
action_info={},
max_time=INF,
max_iterations=INF,
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 action_info: a dictionary from stream name to ActionInfo for planning and execution
: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 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 = float(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)
full_action_info = get_action_info(action_info)
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,
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)
combined_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:
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) 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()
