def visualize_constraints(constraints,
filename='constraint_network' + DEFAULT_EXTENSION,
use_functions=True):
from pygraphviz import AGraph
graph = AGraph(strict=True, directed=False)
graph.node_attr['style'] = 'filled'
#graph.node_attr['fontcolor'] = 'black'
#graph.node_attr['fontsize'] = 12
graph.node_attr['colorscheme'] = 'SVG'
graph.edge_attr['colorscheme'] = 'SVG'
#graph.graph_attr['rotate'] = 90
#graph.node_attr['fixedsize'] = True
graph.node_attr['width'] = 0
graph.node_attr['height'] = 0.02 # Minimum height is 0.02
graph.node_attr['margin'] = 0
graph.graph_attr['rankdir'] = 'RL'
graph.graph_attr['nodesep'] = 0.05
graph.graph_attr['ranksep'] = 0.25
#graph.graph_attr['pad'] = 0
# splines="false";
graph.graph_attr['outputMode'] = 'nodesfirst'
graph.graph_attr['dpi'] = 300
positive, negated, functions = partition_facts(constraints)
for head in (positive + negated + functions):
# TODO: prune values w/o free parameters?
name = str_from_fact(head)
if head in functions:
if not use_functions:
continue
color = COST_COLOR
elif head in negated:
color = NEGATED_COLOR
else:
color = CONSTRAINT_COLOR
graph.add_node(name, shape='box', color=color)
for arg in get_args(head):
if isinstance(arg, OptimisticObject) or is_parameter(arg):
arg_name = str(arg)
graph.add_node(arg_name, shape='circle', color=PARAMETER_COLOR)
graph.add_edge(name, arg_name)
graph.draw(filename, prog='dot') # neato | dot | twopi | circo | fdp | nop
print('Saved', filename)
return graph
def planning_from_satisfaction(init, constraints):
clusters = cluster_constraints(constraints)
prefix = get_internal_prefix(internal=False)
assigned_predicate = ASSIGNED_PREDICATE.format(prefix)
order_predicate = ORDER_PREDICATE.format(prefix)
#order_value_facts = make_order_facts(order_predicate, 0, len(clusters)+1)
order_value_facts = [(order_predicate, '_t{}'.format(i))
for i in range(len(clusters) + 1)]
init.append(order_value_facts[0])
goal_expression = order_value_facts[-1]
order_facts = list(map(obj_from_value_expression, order_value_facts))
bound_parameters = set()
actions = []
#constants = {}
for i, cluster in enumerate(clusters):
objectives = list(map(obj_from_value_expression, cluster.constraints))
constraints, negated, costs = partition_facts(objectives)
if negated:
raise NotImplementedError(negated)
#free_parameters = cluster.parameters - bound_parameters
existing_parameters = cluster.parameters & bound_parameters
# TODO: confirm that negated predicates work as intended
name = 'cluster-{}'.format(i)
parameters = list(sorted(cluster.parameters))
preconditions = [(assigned_predicate, to_constant(p), p) for p in sorted(existing_parameters)] + \
constraints + [order_facts[i]]
effects = [(assigned_predicate, to_constant(p), p) for p in parameters] + \
[order_facts[i+1], Not(order_facts[i])]
if costs:
assert len(costs) == 1
[cost] = costs
else:
cost = None
actions.append(
make_action(name, parameters, preconditions, effects, cost))
#actions[-1].dump()
bound_parameters.update(cluster.parameters)
predicates = [make_predicate(order_predicate, ['?step'])] # '?num',
domain = make_domain(predicates=predicates, actions=actions)
return domain, goal_expression
def constraint_satisfaction(stream_pddl, stream_map, init, terms, stream_info={},
costs=True, max_cost=INF, success_cost=INF, max_time=INF,
unit_efforts=False, max_effort=INF,
max_skeletons=INF, search_sample_ratio=1, verbose=True, **search_args):
# Approaches
# 1) Existential quantification of bindings in goal conditions
# 2) Backtrack useful streams and then schedule. Create arbitrary outputs for not mentioned.
# 3) Construct all useful streams and then associate outputs with bindings
# Useful stream must satisfy at least one fact. How should these assignments be propagated though?
# Make an action that maps each stream result to unbound values?
# TODO: include functions again for cost-sensitive satisfaction
# TODO: convert init into streams to bind certain facts
# TODO: investigate constraint satisfaction techniques for binding instead
# TODO: could also instantiate all possible free parameters even if not useful
# TODO: effort that is a function of the number of output parameters (degrees of freedom)
# TODO: use a CSP solver instead of a planner internally
# TODO: max_iterations?
if not terms:
return {}, 0, init
constraints, negated, functions = partition_facts(set(map(obj_from_existential_expression, terms)))
if not costs:
functions = []
evaluations = evaluations_from_init(init)
goal_facts = set(filter(lambda f: evaluation_from_fact(f) not in evaluations, constraints))
free_parameters = sorted(get_parameters(goal_facts))
print('Parameters:', free_parameters)
externals = parse_stream_pddl(stream_pddl, stream_map, stream_info, unit_efforts=unit_efforts)
stream_results = extract_streams(evaluations, externals, goal_facts)
function_plan = plan_functions(negated + functions, externals)
plan_skeleton = [Assignment(free_parameters)]
cost = get_optimistic_cost(function_plan)
if max_cost < cost:
return None, INF, init
# TODO: detect connected components
# TODO: eagerly evaluate fully bound constraints
# TODO: consider other results if this fails
domain = create_domain(goal_facts)
init_evaluations = evaluations.copy()
store = SolutionStore(evaluations, max_time=max_time, success_cost=success_cost, verbose=verbose)
queue = SkeletonQueue(store, domain, disable=False)
num_iterations = search_time = sample_time = 0
planner = 'ff-astar' # TODO: toggle within reschedule_stream_plan
#last_clusters = set()
#last_success = True
while not store.is_terminated():
num_iterations += 1
start_time = time.time()
print('\nIteration: {} | Skeletons: {} | Skeleton Queue: {} | Evaluations: {} | '
'Cost: {:.3f} | Search Time: {:.3f} | Sample Time: {:.3f} | Total Time: {:.3f}'.format(
num_iterations, len(queue.skeletons), len(queue),
len(evaluations), store.best_cost, search_time, sample_time, store.elapsed_time()))
external_plan = None
if len(queue.skeletons) < max_skeletons:
domain.axioms[:] = create_disabled_axioms(queue, use_parameters=False)
#dominated = are_domainated(last_clusters, clusters)
#last_clusters = clusters
#if last_success or not dominated: # Could also keep a history of results
stream_plan = reschedule_stream_plan(init_evaluations, goal_facts, domain, stream_results,
unique_binding=True, unsatisfiable=True,
max_effort=max_effort, planner=planner, **search_args)
if stream_plan is not None:
external_plan = reorder_stream_plan(combine_optimizers(
init_evaluations, stream_plan + list(function_plan)))
print('Stream plan ({}, {:.3f}): {}'.format(
get_length(external_plan), compute_plan_effort(external_plan), external_plan))
last_success = (external_plan is not None)
search_time += elapsed_time(start_time)
# Once a constraint added for a skeleton, it should only be relaxed
start_time = time.time()
if last_success: # Only works if create_disable_axioms never changes
allocated_sample_time = (search_sample_ratio * search_time) - sample_time
else:
allocated_sample_time = INF
queue.process(external_plan, plan_skeleton, cost=cost,
complexity_limit=INF, max_time=allocated_sample_time)
sample_time += elapsed_time(start_time)
if not last_success and not queue:
break
# TODO: exhaustively compute all plan skeletons and add to queue within the focused algorithm
write_stream_statistics(externals, verbose)
action_plan, cost, facts = revert_solution(store.best_plan, store.best_cost, evaluations)
bindings = bindings_from_plan(plan_skeleton, action_plan)
return bindings, cost, facts
def fn(outputs, facts, hint={}):
# TODO: pass in the variables and constraint streams instead?
# The true test is placing two blocks in a tight region obstructed by one
positive, negative, costs = partition_facts(facts)
#print('Parameters:', outputs)
print('Constraints:', positive + negative)
if costs:
print('Costs:', costs)
model = Model(name='TAMP')
model.setParam(GRB.Param.OutputFlag, verbose)
model.setParam(GRB.Param.TimeLimit, max_time)
var_from_param = {}
for fact in facts:
prefix, args = fact[0], fact[1:]
if prefix == 'conf':
param, = args
if is_parameter(param):
var_from_param[param] = np_var(model)
elif prefix == 'pose':
_, param = args
if is_parameter(param):
var_from_param[param] = np_var(model)
elif prefix == 'traj':
raise NotImplementedError()
#param, = args
#if param not in var_from_id:
# var_from_id[id(param)] = [np_var(model), np_var(model)]
def get_var(p):
return var_from_param[p] if is_parameter(p) else p
objective_terms = []
constraint_from_name = {}
for index, fact in enumerate(facts):
prefix, args = fact[0], fact[1:]
name = str(index)
if prefix == 'kin':
kinematics_constraint(model, name, *map(get_var, args))
elif prefix == 'contained':
contained_constraint(model, regions, name, *map(get_var, args))
elif prefix == 'cfree' and collisions:
collision_constraint(model, name, *map(get_var, args))
elif prefix == 'motion':
#motion_constraint(model, name, *map(get_var, args))
raise NotImplementedError()
elif prefix == NOT:
fact = args[0]
predicate, args = fact[0], fact[1:]
if predicate == 'posecollision' and collisions:
collision_constraint(model, name, *map(get_var, args))
elif prefix == MINIMIZE:
fact = args[0]
func, args = fact[0], fact[1:]
if func == 'distance':
objective_terms.extend(distance_cost(*map(get_var, args)))
continue
constraint_from_name[name] = fact
for out, value in hint.items():
for var, coord in zip(get_var(out), value):
var.start = coord
model.setObjective(quicksum(objective_terms), sense=GRB.MINIMIZE)
#m.write("file.lp")
model.optimize()
# https://www.gurobi.com/documentation/7.5/refman/optimization_status_codes.html
if model.status in (GRB.INFEASIBLE,
GRB.INF_OR_UNBD): # OPTIMAL | SUBOPTIMAL
if not diagnose:
return OptimizerOutput()
constraint_indices = {
i
for i, term in enumerate(facts) if term[0] != MINIMIZE
}
#infeasible = constraint_indices
#infeasible = compute_inconsistent(model)
#infeasible = deletion_filter(model, constraint_indices)
infeasible = elastic_filter(model, constraint_indices)
infeasible_facts = [facts[index] for index in sorted(infeasible)]
print('Inconsistent:', infeasible_facts)
return OptimizerOutput(infeasible=[infeasible])
assignment = tuple(value_from_var(get_var(out)) for out in outputs)
return OptimizerOutput(assignments=[assignment])
def fn(outputs, facts, hint={}):
# TODO: pass in the variables and constraint streams instead?
# The true test is placing two blocks in a tight region obstructed by one
constraint_indices = {
i
for i, term in enumerate(facts) if term[0] != MINIMIZE
}
positive, negative, costs = partition_facts(facts)
#print('Parameters:', outputs)
#print('Constraints:', positive + negative)
if costs:
print('Costs:', costs)
# https://github.com/yijiangh/coop_assembly/blob/e52abef7c1cfb1d3e32691d163abc85dd77f27a2/src/coop_assembly/geometry_generation/caelan.py
model = Model(name='TAMP')
model.setParam(GRB.Param.OutputFlag, verbose)
model.setParam(GRB.Param.TimeLimit, max_time)
model.setParam(GRB.Param.Cutoff,
GRB.INFINITY) # TODO: account for scaling
#if num_solutions < INF:
# model.setParam(GRB.Param.SolutionLimit, num_solutions)
# Limit how many solutions to collect
#model.setParam(GRB.Param.PoolSolutions, 2)
# Limit the search space by setting a gap for the worst possible solution that will be accepted
#model.setParam(GRB.Param.PoolGap, 0.10) # PoolGapAbs
# do a systematic search for the k-best solutions
#model.setParam(GRB.Param.PoolSearchMode, 2) # 0 | 1 | 2
# https://www.gurobi.com/documentation/9.1/examples/poolsearch_py.html#subsubsection:poolsearch.py
##########
# TODO: remove anything that's just a domain condition?
variable_indices = {}
var_from_param = {}
for index, fact in enumerate(facts):
prefix, args = fact[0], fact[1:]
if prefix == 'conf':
param, = args
if is_parameter(param):
var_from_param[param] = np_var(model,
lower=lower,
upper=upper)
elif prefix == 'pose':
_, param = args
if is_parameter(param):
var_from_param[param] = np_var(model,
lower=lower,
upper=upper)
elif prefix == 'grasp': # TODO: iterate over combinations
_, param = args
if is_parameter(param):
var_from_param[param] = GRASP
elif prefix == 'traj':
raise NotImplementedError()
#param, = args
#if param not in var_from_id:
# var_from_id[id(param)] = [np_var(model), np_var(model)]
else:
continue
variable_indices[index] = fact
dimension = sum(len(var) for var in var_from_param.values())
def get_var(p):
return var_from_param[p] if is_parameter(p) else p
##########
codimension = 0
objective_terms = [
] # TODO: could make a variable to impose a cost constraint
constraint_from_name = {}
for index, fact in enumerate(facts):
prefix, args = fact[0], fact[1:]
name = str(index)
if prefix == 'kin':
kinematics_constraint(model, name, *map(get_var, args))
codimension += 2
elif prefix in ('contain', 'contained'):
contained_constraint(model, regions, name, *map(get_var, args))
codimension += 1
elif prefix == 'cfree' and collisions:
# TODO: drop collision constraints until violated
collision_constraint(model, name, *map(get_var, args))
elif prefix == 'motion':
#motion_constraint(model, name, *map(get_var, args))
raise NotImplementedError()
elif prefix == NOT:
fact = args[0]
predicate, args = fact[0], fact[1:]
if predicate == 'posecollision' and collisions:
collision_constraint(model, name, *map(get_var, args))
elif prefix == MINIMIZE:
fact = args[0]
func, args = fact[0], fact[1:]
if func in ('dist', 'distance'):
objective_terms.extend(
distance_cost(model, *map(get_var, args)))
continue
constraint_from_name[name] = fact
model.update()
##########
#linear_model = model
linear_model = copy_model(model)
#linear_model = Model(name='Linear TAMP')
# TODO: prune linearly dependent constraints
linear_constraints = {
c
for c in linear_model.getConstrs() if c.sense == GRB.EQUAL
}
codimension = len(linear_constraints)
# TODO: account for v.LB == v.UB
#linear_variables = {v for v in linear_model.getVars() if v.VType == GRB.CONTINUOUS}
#print(vars_from_expr(linear_model.getObjective()))
linear_variables = set()
for c in linear_constraints:
linear_variables.update(vars_from_expr(linear_model.getRow(c)))
linear_variables = sorted(linear_variables, key=lambda v: v.VarName)
dimension = len(linear_variables)
print('{} variables (dim={}): {}'.format(
len(variable_indices), dimension,
[facts[index] for index in sorted(variable_indices)]))
nontrivial_indices = set(constraint_indices) - set(
variable_indices) # TODO: rename
print('{} constraints: (codim={}): {}'.format(
len(nontrivial_indices), codimension,
[facts[index] for index in sorted(nontrivial_indices)]))
# # https://en.wikipedia.org/wiki/Linear_subspace
# # TODO: Equations for a subspace
# #for c in model.getConstrs():
# # if c.sense != GRB.EQUAL:
# # model.remove(c)
# variables = [model.getVarByName(v.VarName) for v in linear_variables]
# lower_bound = np.array([v.LB for v in variables])
# upper_bound = np.array([v.UB for v in variables])
# center = (lower_bound + upper_bound) / 2.
# extent = (upper_bound - lower_bound) / 2.
# radius = np.linalg.norm(extent) # sphere
#
# point = radius*sample_sphere(dimension) + center
# #point = center
# basis = [sample_sphere_surface(dimension) for _ in range(codimension)]
# #basis = [np.ones(dimension)]
# multipliers = [unbounded_var(model) for _ in basis]
# subspace_constraints = []
# for i in range(dimension):
# combination = sum([m*b[i] for m, b in zip(multipliers, basis)])
# subspace_constraints.append(model.addConstr(variables[i] - point[i] == combination))
# #for c in subspace_constraints:
# # model.remove(c)
# TODO: generator version
# for v in set(linear_model.getVars()) - linear_variables:
# linear_model.remove(v)
# for c in set(linear_model.getConstrs()) - linear_constraints:
# linear_model.remove(c)
# linear_model.setObjective(quicksum(sample_targets(linear_model, linear_variables)), sense=GRB.MINIMIZE)
# linear_model.optimize()
# for v in linear_variables: # Projection method
# set_value(model.getVarByName(v.VarName), v.X)
##########
# TODO: normalize cost relative to the best cost for a trade-off
# TODO: increasing bound on deterioration in quality
weight = 0
if weight > 0:
primary_variables = {
v
for var in var_from_param.values() for v in var
}
objective_terms.extend(
weight * term
for term in sample_targets(model, primary_variables))
model.setObjective(
quicksum(objective_terms),
sense=GRB.MINIMIZE) # (1-weight) * quicksum(objective_terms)
for out, value in hint.items():
for var, coord in zip(get_var(out), value):
# https://www.gurobi.com/documentation/9.1/refman/varhintval.html#attr:VarHintVal
set_guess(var, coord, hard=hard)
#set_value(var, coord)
##########
#m.write("file.lp")
model.optimize()
# https://www.gurobi.com/documentation/7.5/refman/optimization_status_codes.html
#if model.status in (GRB.INFEASIBLE, GRB.INF_OR_UNBD, GRB.CUTOFF): # OPTIMAL | SUBOPTIMAL
if model.SolCount == 0:
if diagnostic is None:
return OptimizerOutput()
elif diagnostic == 'all':
#infeasible = constraint_indices
infeasible = nontrivial_indices
elif diagnostic == 'deletion':
infeasible = deletion_filter(model, constraint_indices)
elif diagnostic == 'elastic':
infeasible = elastic_filter(model, constraint_indices)
elif diagnostic == 'gurobi':
infeasible = compute_inconsistent(model)
else:
raise NotImplementedError(diagnostic)
print('Inconsistent:',
[facts[index] for index in sorted(infeasible)])
return OptimizerOutput(infeasible=[infeasible])
#expr.getValue() # TODO: store expressions and evaluate value
# for c in model.getConstrs():
# print(c, c.Slack, c.RHS)
# print(c.__dict__)
# print(dir(c))
##########
print(
'Solved: {} | Objective: {:.3f} | Solutions: {} | Status: {} | Runtime: {:.3f}'
.format(True, model.ObjVal, model.SolCount, model.status,
model.runtime))
if costs and diagnose_cost:
infeasible = deletion_filter(model,
constraint_indices,
max_objective=model.ObjVal - 1e-6)
else:
# TODO: propagate automatically to optimizer
#infeasible = constraint_indices
infeasible = nontrivial_indices
print('Cost inconsistent:',
[facts[index] for index in sorted(infeasible)])
# variables = list(var_from_param.values())
# for index, solution in enumerate(sample_solutions(model, variables, num_samples=15)):
# print(index, solution)
assignment = tuple(value_from_var(get_var(out)) for out in outputs)
return OptimizerOutput(assignments=[assignment],
infeasible=[infeasible])