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