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