def solve_restart(problem, max_time=INF, max_restarts=0, iteration_time=INF, abort=True, **kwargs):
# TODO: iteratively lower the cost bound
# TODO: a sequence of different planner configurations
# TODO: reset objects and/or streams
if (max_restarts >= 1) and (iteration_time == INF):
iteration_time = min(2 * 60, iteration_time)
assert (max_restarts == 0) or (iteration_time != INF)
assert max_restarts >= 0
start_time = time.time()
for attempt in irange(1+max_restarts):
iteration_start_time = time.time()
if elapsed_time(start_time) > max_time:
break
if attempt >= 1:
print(SEPARATOR)
#solution = planner_fn(problem) # Or include the problem in the lambda
remaining_time = min(iteration_time, max_time-elapsed_time(start_time))
solution = solve(problem, max_time=remaining_time, **kwargs)
plan, cost, certificate = solution
if is_plan(plan): # TODO: INFEASIBLE
return solution
if abort and (elapsed_time(iteration_start_time) < remaining_time):
break # TODO: return the cause of failure
certificate = Certificate(all_facts=[], preimage_facts=[]) # TODO: aggregate
return Solution(None, INF, certificate)
def gen_fn(*input_values):
input_objects = stream_instance.input_objects
mapping = get_mapping(stream.inputs, input_objects)
selected_objects = tuple(mapping[inp] for inp in inputs)
#for _ in irange(self.num):
for _ in irange(stream_instance.num_optimistic):
yield [tuple(OptValue(stream.name, inputs, selected_objects, out)
for out in stream.outputs)]
def gen_fn(*args, **kwargs):
for _ in irange(max_attempts):
if random.random() < p_success:
values = ['{}-{}'.format(output[1:], len(history)) for output in outputs]
history.append(values)
yield values
else:
yield None
def gen_fn(*input_values):
if not self.test(*input_values):
return
# TODO: recover input_objects from input_values
selected_objects = select_inputs(stream_instance, inputs)
#for _ in irange(self.num):
for _ in irange(stream_instance.num_optimistic):
yield [tuple(SharedOptValue(stream.name, inputs, selected_objects, out)
for out in stream.outputs)]
def gen_fn(*input_values):
input_objects = tuple(map(Object.from_value, input_values))
instance = stream.get_instance(input_objects)
mapping = get_mapping(stream.inputs, input_objects)
values = tuple(mapping[inp] for inp in inputs)
assert(len(inputs) == len(values))
#for _ in irange(self.num):
for _ in irange(instance.num_optimistic):
yield [tuple(OptValue(stream.name, inputs, values, out)
for out in stream.outputs)]
def gen_fn(*input_values):
if not self.test(*input_values):
return
# TODO: recover input_objects from input_values
selected_objects = select_inputs(instance, inputs)
for idx in irange(instance.num_optimistic): # self.num
# if len(inputs) == len(external.inputs):
# yield [tuple(UniqueOptValue(instance, idx, out)
# for out in external.outputs)]
# else:
yield [
tuple(
SharedOptValue(external.name, inputs, selected_objects,
out) for out in external.outputs)
]
def gen_fn(node1, element): # fluents=[]):
reverse = (node1 != element[0])
element_body = element_bodies[element]
n1, n2 = reversed(element) if reverse else element
delta = node_points[n2] - node_points[n1]
# if delta[2] < 0:
# continue
length = np.linalg.norm(delta) # 5cm
#supporters = {e for e in node_neighbors[n1] if element_supports(e, n1, node_points)}
supporters = []
retrace_supporters(element, incoming_supporters, supporters)
elements_order = [
e for e in element_bodies
if (e != element) and (e not in supporters)
]
bodies_order = [element_bodies[e] for e in elements_order]
obstacles = fixed_obstacles + [element_bodies[e] for e in supporters]
collision_fn = get_collision_fn(
robot,
movable_joints,
obstacles,
attachments=[],
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
custom_limits={}) # TODO: get_custom_limits
trajectories = []
for num in irange(max_trajectories):
for attempt in range(max_attempts):
path = sample_print_path(robot, length, reverse, element_body,
collision_fn)
if path is None:
continue
if check_collisions:
collisions = check_trajectory_collision(
robot, path, bodies_order)
colliding = {
e
for k, e in enumerate(elements_order)
if (element != e) and collisions[k]
}
else:
colliding = set()
if (node_neighbors[n1] <= colliding) and not any(
n in ground_nodes for n in element):
continue
print_traj = PrintTrajectory(robot, movable_joints, path,
element, reverse, colliding)
trajectories.append(print_traj)
# TODO: need to prune dominated trajectories
if print_traj not in trajectories:
continue
print('{}) {}->{} ({}) | {} | {} | {}'.format(
num, n1, n2, len(supporters), attempt, len(trajectories),
sorted(len(t.colliding) for t in trajectories)))
yield (print_traj, )
if not colliding:
return
else:
print('{}) {}->{} ({}) | {} | Max attempts exceeded!'.format(
num, len(supporters), n1, n2, max_attempts))
user_input('Continue?')
return
def gen_fn(node1, element, extruded=[], trajectories=[]): # fluents=[]):
start_time = time.time()
assert not is_reversed(element_bodies, element)
idle_time = 0
reverse = is_end(node1, element)
if disable or (len(extruded) < SKIP_PERCENTAGE *
len(element_bodies)): # For quick visualization
path, tool_path = [], []
traj = PrintTrajectory(end_effector, get_movable_joints(robot),
path, tool_path, element, reverse)
command = Command([traj])
yield (command, )
return
directed = reverse_element(element) if reverse else element
extrusion = extrusions[directed]
n1, n2 = reverse_element(element) if reverse else element
neighboring_elements = node_neighbors[n1] & node_neighbors[n2]
supporters = [] # TODO: can also do according to levels
retrace_supporters(element, incoming_supporters, supporters)
element_obstacles = {
element_bodies[e]
for e in supporters + list(extruded)
}
obstacles = set(fixed_obstacles) | element_obstacles
if not collisions:
obstacles = set()
#obstacles = set(fixed_obstacles)
elements_order = [
e for e in element_bodies if (e != element) and (
e not in removed) and (element_bodies[e] not in obstacles)
]
collision_fn = get_element_collision_fn(robot, obstacles)
#print(len(fixed_obstacles), len(element_obstacles), len(elements_order))
trajectories = list(trajectories)
for num in irange(INF):
for attempt, tool_traj in enumerate(
islice(extrusion.generator(), max_directions)):
# TODO: is this slower now for some reason?
#tool_traj.safe_from_body = {} # Disables caching
if not tool_traj.is_safe(obstacles):
continue
for _ in range(max_attempts):
if max_time <= elapsed_time(start_time):
return
nearby_conf = initial_conf if random.random(
) < p_nearby else None
command = compute_direction_path(tool_traj,
collision_fn,
ee_only=ee_only,
initial_conf=nearby_conf,
**kwargs)
if command is None:
continue
command.update_safe(extruded)
if precompute_collisions:
bodies_order = [
element_bodies[e] for e in elements_order
]
colliding = command_collision(end_effector, command,
bodies_order)
for element2, unsafe in zip(elements_order, colliding):
if unsafe:
command.set_unsafe(element2)
else:
command.set_safe(element2)
if not is_ground(element, ground_nodes) and (
neighboring_elements <= command.colliding):
continue # If all neighbors collide
trajectories.append(command)
if precompute_collisions:
prune_dominated(trajectories)
if command not in trajectories:
continue
print(
'{}) {}->{} | EE: {} | Supporters: {} | Attempts: {} | Trajectories: {} | Colliding: {}'
.format(num, n1, n2, ee_only, len(supporters), attempt,
len(trajectories),
sorted(len(t.colliding)
for t in trajectories)))
temp_time = time.time()
yield (command, )
idle_time += elapsed_time(temp_time)
if precompute_collisions:
if len(command.colliding) == 0:
#print('Reevaluated already non-colliding trajectory!')
return
elif len(command.colliding) == 1:
[colliding_element] = command.colliding
obstacles.add(element_bodies[colliding_element])
break
else:
if allow_failures:
yield None
else:
print(
'{}) {}->{} | EE: {} | Supporters: {} | Attempts: {} | Max attempts exceeded!'
.format(num, n1, n2, ee_only, len(supporters),
max_directions))
return