def main(deterministic=False, observable=False, collisions=True, factor=True): parser = create_parser() args = parser.parse_args() print('Arguments:', args) # TODO: AssertionError: degenerate distribution DDist{l0: 0.00000} # TODO: global search over the state belief_problem = get_belief_problem(deterministic, observable) pddlstream_problem = to_pddlstream(belief_problem, collisions) print('Cost scale:', get_cost_scale()) stream_info = { 'GE': StreamInfo(from_test(ge_fn), eager=False), 'prob-after-move': StreamInfo(from_fn(get_opt_move_fn(factor=factor))), 'MoveCost': FunctionInfo(move_cost_fn), 'prob-after-look': StreamInfo(from_fn(get_opt_obs_fn(factor=factor))), 'LookCost': FunctionInfo(get_look_cost_fn(p_look_fp=0, p_look_fn=0)), } planner = 'ff-wastar1' success_cost = 0 # 0 | MAX_COST with Profiler(field='tottime', num=10): solution = solve(pddlstream_problem, algorithm=args.algorithm, unit_costs=args.unit, stream_info=stream_info, planner=planner, debug=False, success_cost=success_cost, max_time=30) print_solution(solution)
def get_plan_cost(action_plan, cost_from_action): if action_plan is None: return INF # TODO: return cost per action instance #return sum([0.] + [instance.cost for instance in action_plan]) scaled_cost = sum([0.] + [cost_from_action[instance] for instance in action_plan]) return scaled_cost / get_cost_scale()
def get_plan_cost(action_plan, cost_from_action, unit_costs): if action_plan is None: return INF if unit_costs: # TODO: no longer need to pass around unit_costs return len(action_plan) #return sum([0.] + [instance.cost for instance in action_plan]) scaled_cost = sum([0.] + [cost_from_action[instance] for instance in action_plan]) return scaled_cost / get_cost_scale()
def get_action_cost(domain, results_from_head, name, args): import pddl action = find_unique(lambda a: a.name == name, domain.actions) if action.cost is None: return 0. if isinstance(action.cost.expression, pddl.NumericConstant): return action.cost.expression.value / get_cost_scale() var_mapping = {p.name: a for p, a in zip(action.parameters, args)} args = tuple(var_mapping[p] for p in action.cost.expression.args) head = Head(action.cost.expression.symbol, args) [(value, _)] = results_from_head[head] return value
def main(deterministic=False, observable=False, collisions=True, focused=True, factor=True): # TODO: global search over the state belief_problem = get_belief_problem(deterministic, observable) pddlstream_problem = to_pddlstream(belief_problem, collisions) set_cost_scale(SCALE_COST) print('Cost scale:', get_cost_scale()) pr = cProfile.Profile() pr.enable() planner = 'ff-wastar1' if focused: stream_info = { 'GE': StreamInfo(from_test(ge_fn), eager=False), 'prob-after-move': StreamInfo(from_fn(get_opt_move_fn(factor=factor))), 'MoveCost': FunctionInfo(move_cost_fn), 'prob-after-look': StreamInfo(from_fn(get_opt_obs_fn(factor=factor))), 'LookCost': FunctionInfo(get_look_cost_fn(p_look_fp=0, p_look_fn=0)), } solution = solve_focused(pddlstream_problem, stream_info=stream_info, planner=planner, debug=False, success_cost=0, unit_costs=False, max_time=30) else: solution = solve_incremental(pddlstream_problem, planner=planner, debug=True, success_cost=MAX_COST, unit_costs=False, max_time=30) pr.disable() pstats.Stats(pr).sort_stats('tottime').print_stats(10) print_solution(solution) plan, cost, init = solution print('Real cost:', cost)
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))
locations = {l0} if deterministic: p_move_s = 1.0 p_look_fp = p_look_fn = 0.0 else: p_move_s = 0.8 p_look_fp = p_look_fn = 0.1 return BeliefProblem(initial, goal, locations, p_move_s, p_look_fn, p_look_fp) ################################################## MAX_COST = MAX_FD_COST / (100 * get_cost_scale()) def clip_cost(cost, max_cost=MAX_COST): # TODO: move this to downward? if cost == INF: return max_cost return min(cost, max_cost) #def clip_p(p, min_p=1e-3, max_p=1-1e-3): # return min(max(min_p, p), max_p) #def log_cost(p): # if p == 0: # return INF # return -math.log(p)
def solve_pyplanners(instantiated, planner=None, max_planner_time=DEFAULT_MAX_TIME, max_cost=INF): if instantiated is None: return None, INF # https://github.mit.edu/caelan/stripstream/blob/c8c6cd1d6bd5e2e8e31cd5603e28a8e0d7bb2cdc/stripstream/algorithms/search/pyplanners.py pyplanners_path = get_pyplanners_path() if pyplanners_path is None: raise RuntimeError( 'Must clone https://github.com/caelan/pyplanners ' 'and set the environment variable {} to its path'.format( PYPLANNERS_VAR)) if pyplanners_path not in sys.path: sys.path.append(pyplanners_path) # TODO: could operate on translated SAS instead from strips.states import State, PartialState from strips.operators import Action, Axiom from strips.utils import solve_strips, default_derived_plan import pddl # TODO: PLUSONE costs pyplanner = dict(DEFAULT_PYPLANNER) if isinstance(planner, dict): pyplanner.update(planner) fd_action_from_py_action = {} py_actions = [] for action in instantiated.actions: #action.dump() py_action = Action({'fd_action': action}) py_action.conditions = set(action.precondition) py_action.effects = set() for condition, effect in action.del_effects: assert not condition py_action.effects.add(effect.negate()) for condition, effect in action.add_effects: assert not condition py_action.effects.add(effect) py_action.cost = action.cost py_action.test, fd_action_from_py_action[ py_action] = get_attachment_test(action) py_actions.append(py_action) py_axioms = [] for axiom in instantiated.axioms: #axiom.dump() py_axiom = Axiom({'fd_axiom_id': id(axiom)}) # Not hashable for some reason py_axiom.conditions = set(axiom.condition) py_axiom.effects = {axiom.effect} py_axioms.append(py_axiom) goal = PartialState(instantiated.goal_list) fluents = {f.positive() for f in goal.conditions} for py_operator in py_actions + py_axioms: fluents.update(f.positive() for f in py_operator.conditions) initial = State(atom for atom in instantiated.task.init if isinstance(atom, pddl.Atom) and (atom in fluents)) plan, state_space = solve_strips(initial, goal, py_actions, py_axioms, max_time=max_planner_time, max_cost=max_cost, **pyplanner) if plan is None: return None, INF #fd_plan = [action.fd_action for action in plan.operators] states = plan.get_states() # get_states | get_derived_states fd_plan = [ fd_action_from_py_action[action][state] for state, action in safe_zip(states[:-1], plan.operators) ] actions = [pddl_from_instance(action) for action in fd_plan] #print(actions) cost = plan.cost / get_cost_scale() return actions, cost