def sample_trajectories(robot, obstacles, node_points, element_bodies, ground_nodes):
gen_fn = get_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes)
all_trajectories = []
for index, (element, element_body) in enumerate(element_bodies.items()):
label_element(element_bodies, element)
trajectories = []
for node1 in element:
for traj, in gen_fn(node1, element):
trajectories.append(traj)
all_trajectories.extend(trajectories)
if not trajectories:
return None
return all_trajectories
def optimize_commands(robot, obstacles, element_bodies, extrusion_path, initial_conf, commands,
max_iterations=INF, max_time=60,
motions=True, collisions=True, **kwargs):
if commands is None:
return None
start_time = time.time()
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path)
print_gen_fn = get_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes,
precompute_collisions=False, collisions=collisions, **kwargs)
commands = list(commands)
sequence = [command.elements[0] for command in commands]
directed_sequence = [command.directed_elements[0] for command in commands]
indices = list(range(len(sequence)))
#trajectories = flatten_commands(commands)
#print(len(trajectories), get_print_distance(trajectories, teleport=False))
total_distance = sum(get_command_distance(robot, initial_conf, commands, index, command)
for index, command in enumerate(commands))
# TODO: bias sampling towards far apart relative to neighoors
# TODO: bias IK solutions to be the best of several
iterations = extrusion_failures = 0
while (iterations < max_iterations) and (elapsed_time(start_time) < max_time):
iterations += 1
index = random.choice(indices)
printed = sequence[:index-1]
element = sequence[index]
directed = directed_sequence[index]
print(commands[index])
distance = get_command_distance(robot, initial_conf, commands, index, commands[index])
print('Iteration {} | Failures: {} | Distance: {:.3f} | Index: {}/{} | Time: {:.3f}'.format(
iterations, extrusion_failures, total_distance, index, len(indices), elapsed_time(start_time)))
new_command, = next(print_gen_fn(directed[0], element, extruded=printed), (None,))
if new_command is None:
extrusion_failures += 1
continue
new_distance = get_command_distance(robot, initial_conf, commands, index, new_command)
if new_distance < distance:
commands.pop(index)
commands.insert(index, new_command)
total_distance -= (distance - new_distance)
# data = {
# 'extrusion_failures': extrusion_failures,
# }
return commands #, data
def get_fluent_print_gen_fn(robots,
static_obstacles,
node_points,
element_bodies,
ground_nodes,
connectivity=False,
stiffness=False,
debug=True,
**kwargs):
#wild_print_gen_fn = get_wild_print_gen_fn(robots, static_obstacles, node_points, element_bodies, ground_nodes,
# initial_confs={}, return_home=False, **kwargs) # collisions=False,
print_gen_fn_from_robot = {
robot: get_print_gen_fn(robot,
static_obstacles,
node_points,
element_bodies,
ground_nodes,
precompute_collisions=False,
**kwargs)
for robot in robots
}
test_printable = get_test_printable(ground_nodes, debug=debug)
#test_stiff = get_test_stiff(debug=debug)
def gen_fn(name, node1, element, node2, fluents=[]):
robot = index_from_name(robots, name)
printed = extract_printed(fluents)
next_printed = printed - {element}
if connectivity and not test_printable(node1, element,
fluents=fluents):
return
if stiffness and not test_stiffness(
extrusion_path, element_from_id, next_printed,
checker=checker):
return
generator = print_gen_fn_from_robot[robot](node1,
element,
extruded=next_printed)
#generator = islice(generator, stop=1)
#return generator
for print_cmd, in generator:
yield (print_cmd, )
#break
return gen_fn
def get_wild_print_gen_fn(robots,
static_obstacles,
node_points,
element_bodies,
ground_nodes,
initial_confs={},
return_home=False,
collisions=True,
**kwargs):
# TODO: could reuse end-effector trajectories
# TODO: max distance from nearby
gen_fn_from_robot = {
robot: get_print_gen_fn(robot,
static_obstacles,
node_points,
element_bodies,
ground_nodes,
p_nearby=1.,
approach_distance=0.05,
precompute_collisions=True,
**kwargs)
for robot in robots
}
wild_move_fn = get_wild_move_gen_fn(robots, static_obstacles,
element_bodies, **kwargs)
def wild_gen_fn(name, node1, element, node2):
# TODO: could cache this
# sequence = [result.get_mapping()['?e'].value for result in CURRENT_STREAM_PLAN]
# index = sequence.index(element)
# printed = sequence[:index]
# TODO: this might need to be recomputed per iteration
# TODO: condition on plan/downstream constraints
# TODO: stream fusion
# TODO: split element into several edges
robot = index_from_name(robots, name)
q0 = initial_confs[name]
#generator = gen_fn_from_robot[robot](node1, element)
for print_cmd, in gen_fn_from_robot[robot](node1, element):
# TODO: need to merge safe print_cmd.colliding
q1 = Conf(robot, print_cmd.start_conf, node=node1, element=element)
q2 = Conf(robot, print_cmd.end_conf, node=node2, element=element)
if return_home:
# TODO: can decompose into individual movements as well
output1 = next(wild_move_fn(name, q0, q1), None)
if not output1:
continue
transit_cmd1 = output1.values[0][0]
print_cmd.trajectories = transit_cmd1.trajectories + print_cmd.trajectories
output2 = next(wild_move_fn(name, q2, q0), None)
if not output2:
continue
transit_cmd2 = output2.values[0][0]
print_cmd.trajectories = print_cmd.trajectories + transit_cmd2.trajectories
q1 = q2 = q0 # TODO: must assert that AtConf holds
outputs = [(q1, q2, print_cmd)]
# Prevents premature collision checks
facts = [('CTraj', name, print_cmd)
] # + [('Dummy',)] # To force to be wild
if collisions:
facts.extend(
('Collision', print_cmd, e2) for e2 in print_cmd.colliding)
yield WildOutput(outputs, facts)
return wild_gen_fn
def regression(robot,
obstacles,
element_bodies,
extrusion_path,
partial_orders=[],
heuristic='z',
max_time=INF,
max_memory=INF,
backtrack_limit=INF,
revisit=False,
stiffness_attempts=1,
collisions=True,
stiffness=True,
motions=True,
lazy=LAZY,
checker=None,
**kwargs):
# Focused has the benefit of reusing prior work
# Greedy has the benefit of conditioning on previous choices
# TODO: max branching factor
# TODO: be more careful when near the end
# TODO: max time spent evaluating successors (less expensive when few left)
# TODO: tree rollouts
# TODO: best-first search with a minimizing path distance cost
# TODO: immediately select if becomes more stable
# TODO: focus branching factor on most stable regions
start_time = time.time()
initial_conf = get_configuration(robot)
initial_position = get_tool_position(robot)
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path)
id_from_element = get_id_from_element(element_from_id)
all_elements = frozenset(element_bodies)
ground_elements = get_ground_elements(all_elements, ground_nodes)
if stiffness and (checker is None):
checker = create_stiffness_checker(extrusion_path, verbose=False)
print_gen_fn = get_print_gen_fn(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
precompute_collisions=False,
max_directions=MAX_DIRECTIONS,
max_attempts=MAX_ATTEMPTS,
collisions=collisions,
**kwargs)
heuristic_fn = get_heuristic_fn(robot,
extrusion_path,
heuristic,
checker=checker,
forward=False)
queue = []
outgoing_from_element = outgoing_from_edges(partial_orders)
def add_successors(printed, position, conf):
only_ground = printed <= ground_elements
num_remaining = len(printed) - 1
#assert 0 <= num_remaining
for element in randomize(printed):
if not (outgoing_from_element[element] & printed) and implies(
is_ground(element, ground_nodes), only_ground):
for directed in get_directions(element):
visits = 0
bias = heuristic_fn(printed, directed, position, conf)
priority = (num_remaining, bias, random.random())
heapq.heappush(queue,
(visits, priority, printed, directed, conf))
final_conf = initial_conf # TODO: allow choice of final config
final_position = initial_position
final_printed = all_elements
visited = {final_printed: Node(None, None)}
if check_connected(ground_nodes, final_printed) and \
(not stiffness or test_stiffness(extrusion_path, element_from_id, final_printed, checker=checker)):
add_successors(final_printed, final_position, final_conf)
# if has_gui():
# sequence = sorted(final_printed, key=lambda e: heuristic_fn(final_printed, e, conf=None), reverse=True)
# remove_all_debug()
# draw_ordered(sequence, node_points)
# wait_for_user()
plan = None
min_remaining = len(all_elements)
num_evaluated = max_backtrack = extrusion_failures = transit_failures = stiffness_failures = 0
while queue and (elapsed_time(start_time) <
max_time) and check_memory(): #max_memory):
visits, priority, printed, directed, current_conf = heapq.heappop(
queue)
element = get_undirected(all_elements, directed)
num_remaining = len(printed)
backtrack = num_remaining - min_remaining
max_backtrack = max(max_backtrack, backtrack)
if backtrack_limit < backtrack:
break # continue
num_evaluated += 1
print(
'Iteration: {} | Best: {} | Printed: {} | Element: {} | Index: {} | Time: {:.3f}'
.format(num_evaluated, min_remaining, len(printed), element,
id_from_element[element], elapsed_time(start_time)))
next_printed = printed - {element}
next_nodes = compute_printed_nodes(ground_nodes, next_printed)
#draw_action(node_points, next_printed, element)
#if 3 < backtrack + 1:
# remove_all_debug()
# set_renderer(enable=True)
# draw_model(next_printed, node_points, ground_nodes)
# wait_for_user()
node1, node2 = directed
if (next_printed
in visited) or (node1
not in next_nodes) or not check_connected(
ground_nodes, next_printed):
continue
# TODO: stiffness plan lazily here possibly with reuse
if stiffness and not test_stiffness(
extrusion_path, element_from_id, next_printed,
checker=checker):
stiffness_failures += 1
continue
# TODO: stronger condition for this procedure
# if plan_stiffness(extrusion_path, element_from_id, node_points, ground_nodes, next_printed,
# checker=checker, max_backtrack=0) is None:
# # TODO: cache and reuse prior stiffness plans
# print('Failed stiffness plan') # TODO: require just a short horizon
# continue
if revisit:
heapq.heappush(
queue, (visits + 1, priority, printed, directed, current_conf))
command, = next(print_gen_fn(node1, element, extruded=next_printed),
(None, ))
if command is None:
extrusion_failures += 1
continue
if motions and not lazy:
motion_traj = compute_motion(
robot,
obstacles,
element_bodies,
printed,
command.end_conf,
current_conf,
collisions=collisions,
max_time=max_time -
elapsed_time(start_time)) # TODO: smooth=...)
if motion_traj is None:
transit_failures += 1
continue
command.trajectories.append(motion_traj)
if num_remaining < min_remaining:
min_remaining = num_remaining
#print('New best: {}'.format(num_remaining))
#if has_gui():
# # TODO: change link transparency
# remove_all_debug()
# draw_model(next_printed, node_points, ground_nodes)
# wait_for_duration(0.5)
visited[next_printed] = Node(
command, printed) # TODO: be careful when multiple trajs
if not next_printed:
min_remaining = 0
commands = retrace_commands(visited, next_printed, reverse=True)
if OPTIMIZE:
commands = optimize_commands(robot,
obstacles,
element_bodies,
extrusion_path,
initial_conf,
commands,
motions=motions,
collisions=collisions)
plan = flatten_commands(commands)
if motions and not lazy:
motion_traj = compute_motion(robot,
obstacles,
element_bodies,
frozenset(),
initial_conf,
plan[0].start_conf,
collisions=collisions,
max_time=max_time -
elapsed_time(start_time))
if motion_traj is None:
plan = None
transit_failures += 1
else:
plan.insert(0, motion_traj)
if motions and lazy:
plan = compute_motions(robot,
obstacles,
element_bodies,
initial_conf,
plan,
collisions=collisions,
max_time=max_time -
elapsed_time(start_time))
break
# if plan is not None:
# break
add_successors(next_printed, node_points[node1], command.start_conf)
#del checker
data = {
#'memory': get_memory_in_kb(), # May need to update instead
'num_evaluated': num_evaluated,
'min_remaining': min_remaining,
'max_backtrack': max_backtrack,
'stiffness_failures': stiffness_failures,
'extrusion_failures': extrusion_failures,
'transit_failures': transit_failures,
}
return plan, data
def progression(robot,
obstacles,
element_bodies,
extrusion_path,
partial_orders=[],
heuristic='z',
max_time=INF,
backtrack_limit=INF,
revisit=False,
stiffness=True,
motions=True,
collisions=True,
lazy=LAZY,
checker=None,
**kwargs):
start_time = time.time()
initial_conf = get_configuration(robot)
initial_position = get_tool_position(robot)
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path)
if checker is None:
checker = create_stiffness_checker(extrusion_path, verbose=False)
print_gen_fn = get_print_gen_fn(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
precompute_collisions=False,
collisions=collisions,
**kwargs)
id_from_element = get_id_from_element(element_from_id)
all_elements = frozenset(element_bodies)
heuristic_fn = get_heuristic_fn(robot,
extrusion_path,
heuristic,
checker=checker,
forward=True)
initial_printed = frozenset()
queue = []
visited = {initial_printed: Node(None, None)}
if check_connected(ground_nodes, all_elements) and \
test_stiffness(extrusion_path, element_from_id, all_elements):
add_successors(queue,
all_elements,
node_points,
ground_nodes,
heuristic_fn,
initial_printed,
initial_position,
initial_conf,
partial_orders=partial_orders)
plan = None
min_remaining = len(all_elements)
num_evaluated = max_backtrack = stiffness_failures = extrusion_failures = transit_failures = 0
while queue and (elapsed_time(start_time) < max_time):
num_evaluated += 1
visits, priority, printed, directed, current_conf = heapq.heappop(
queue)
element = get_undirected(all_elements, directed)
num_remaining = len(all_elements) - len(printed)
backtrack = num_remaining - min_remaining
max_backtrack = max(max_backtrack, backtrack)
if backtrack_limit < backtrack:
break # continue
num_evaluated += 1
if num_remaining < min_remaining:
min_remaining = num_remaining
print(
'Iteration: {} | Best: {} | Printed: {} | Element: {} | Index: {} | Time: {:.3f}'
.format(num_evaluated, min_remaining, len(printed), element,
id_from_element[element], elapsed_time(start_time)))
next_printed = printed | {element}
assert check_connected(ground_nodes, next_printed)
if (next_printed in visited) or (stiffness and not test_stiffness(
extrusion_path, element_from_id, next_printed, checker=checker)
):
stiffness_failures += 1
continue
if revisit: # could also prevent revisiting if command is not None
heapq.heappush(
queue, (visits + 1, priority, printed, directed, current_conf))
node1, node2 = directed
command, = next(print_gen_fn(node1, element, extruded=printed),
(None, ))
if command is None:
extrusion_failures += 1
continue
if motions and not lazy:
# TODO: test reachability from initial_conf
motion_traj = compute_motion(robot,
obstacles,
element_bodies,
printed,
current_conf,
command.start_conf,
collisions=collisions,
max_time=max_time -
elapsed_time(start_time))
if motion_traj is None:
transit_failures += 1
continue
command.trajectories.insert(0, motion_traj)
visited[next_printed] = Node(command, printed)
if all_elements <= next_printed:
min_remaining = 0
commands = retrace_commands(visited, next_printed)
if OPTIMIZE:
commands = optimize_commands(robot,
obstacles,
element_bodies,
extrusion_path,
initial_conf,
commands,
motions=motions,
collisions=collisions)
plan = flatten_commands(commands)
if motions and not lazy:
motion_traj = compute_motion(robot,
obstacles,
element_bodies,
frozenset(),
initial_conf,
plan[0].start_conf,
collisions=collisions,
max_time=max_time -
elapsed_time(start_time))
if motion_traj is None:
plan = None
transit_failures += 1
else:
plan.append(motion_traj)
if motions and lazy:
plan = compute_motions(robot,
obstacles,
element_bodies,
initial_conf,
plan,
collisions=collisions,
max_time=max_time -
elapsed_time(start_time))
break
# if plan is not None:
# break
add_successors(queue,
all_elements,
node_points,
ground_nodes,
heuristic_fn,
next_printed,
node_points[node2],
command.end_conf,
partial_orders=partial_orders)
data = {
'num_evaluated': num_evaluated,
'min_remaining': min_remaining,
'max_backtrack': max_backtrack,
'stiffness_failures': stiffness_failures,
'extrusion_failures': extrusion_failures,
'transit_failures': transit_failures,
}
return plan, data
def lookahead(robot,
obstacles,
element_bodies,
extrusion_path,
partial_orders=[],
num_ee=0,
num_arm=1,
plan_all=False,
use_conflicts=False,
use_replan=False,
heuristic='z',
max_time=INF,
backtrack_limit=INF,
revisit=False,
ee_only=False,
collisions=True,
stiffness=True,
motions=True,
lazy=LAZY,
checker=None,
**kwargs):
if not use_conflicts:
num_ee, num_arm = min(num_ee, 1), min(num_arm, 1)
if ee_only:
num_ee, num_arm = max(num_arm, num_ee), 0
print('#EE: {} | #Arm: {}'.format(num_ee, num_arm))
# TODO: only check nearby remaining_elements
# TODO: only check collisions conditioned on current decisions
start_time = time.time()
initial_conf = get_configuration(robot)
initial_position = get_tool_position(robot)
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path)
if checker is None:
checker = create_stiffness_checker(extrusion_path, verbose=False)
#print_gen_fn = get_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes,
# precompute_collisions=False, supports=False, ee_only=ee_only,
# max_directions=MAX_DIRECTIONS, max_attempts=MAX_ATTEMPTS, collisions=collisions, **kwargs)
full_print_gen_fn = get_print_gen_fn(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
precompute_collisions=False,
ee_only=ee_only,
allow_failures=True,
collisions=collisions,
**kwargs)
# TODO: could just check environment collisions & kinematics instead of element collisions
ee_print_gen_fn = get_print_gen_fn(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
precompute_collisions=False,
ee_only=True,
allow_failures=True,
collisions=collisions,
**kwargs)
id_from_element = get_id_from_element(element_from_id)
all_elements = frozenset(element_bodies)
heuristic_fn = get_heuristic_fn(robot,
extrusion_path,
heuristic,
checker=checker,
forward=True)
#distance_fn = get_distance_fn(robot, joints, weights=JOINT_WEIGHTS)
# TODO: 2-step lookahead based on neighbors or spatial proximity
full_sample_traj, full_trajs_from_element = get_sample_traj(
ground_nodes, element_bodies, full_print_gen_fn, collisions=collisions)
ee_sample_traj, ee_trajs_from_element = get_sample_traj(
ground_nodes, element_bodies, ee_print_gen_fn, collisions=collisions)
if ee_only:
full_sample_traj = ee_sample_traj
#ee_sample_traj, ee_trajs_from_element = full_sample_traj, full_trajs_from_element
#heuristic_trajs_from_element = full_trajs_from_element if (num_ee == 0) else ee_trajs_from_element
heuristic_trajs_from_element = full_trajs_from_element if (
num_arm != 0) else ee_trajs_from_element
#########################
def sample_remaining(printed, next_printed, sample_fn, num=1, **kwargs):
if num == 0:
return True
remaining_elements = (all_elements - next_printed) if plan_all else \
compute_printable_elements(all_elements, ground_nodes, next_printed)
# TODO: could just consider nodes in printed (connected=True)
return all(
sample_fn(printed,
next_printed,
element,
connected=False,
num=num,
**kwargs) for element in randomize(remaining_elements))
def conflict_fn(printed, element, conf):
# Dead-end detection without stability performs reasonably well
# TODO: could add element if desired
order = retrace_elements(visited, printed)
printed = frozenset(
order[:-1]) # Remove last element (to ensure at least one traj)
if use_replan:
remaining = list(all_elements - printed)
requires_replan = [
all(element in traj.colliding
for traj in ee_trajs_from_element[e2]
if not (traj.colliding & printed)) for e2 in remaining
if e2 != element
]
return len(requires_replan)
else:
safe_trajectories = [
traj for traj in heuristic_trajs_from_element[element]
if not (traj.colliding & printed)
]
assert safe_trajectories
best_traj = max(safe_trajectories,
key=lambda traj: len(traj.colliding))
num_colliding = len(best_traj.colliding)
return -num_colliding
#distance = distance_fn(conf, best_traj.start_conf)
# TODO: ee distance vs conf distance
# TODO: l0 distance based on whether we remain at the same node
# TODO: minimize instability while printing (dynamic programming)
#return (-num_colliding, distance)
if use_conflicts:
priority_fn = lambda *args: (conflict_fn(*args), heuristic_fn(*args))
else:
priority_fn = heuristic_fn
#########################
initial_printed = frozenset()
queue = []
visited = {initial_printed: Node(None, None)}
if check_connected(ground_nodes, all_elements) and \
test_stiffness(extrusion_path, element_from_id, all_elements) and \
sample_remaining(initial_printed, initial_printed, ee_sample_traj, num=num_ee) and \
sample_remaining(initial_printed, initial_printed, full_sample_traj, num=num_arm):
add_successors(queue,
all_elements,
node_points,
ground_nodes,
priority_fn,
initial_printed,
initial_position,
initial_conf,
partial_orders=partial_orders)
plan = None
min_remaining = INF
num_evaluated = worst_backtrack = num_deadends = stiffness_failures = extrusion_failures = transit_failures = 0
while queue and (elapsed_time(start_time) < max_time):
num_evaluated += 1
visits, priority, printed, directed, current_conf = heapq.heappop(
queue)
element = get_undirected(all_elements,
directed) # TODO: use the correct direction
num_remaining = len(all_elements) - len(printed)
backtrack = num_remaining - min_remaining
worst_backtrack = max(worst_backtrack, backtrack)
if backtrack_limit < backtrack:
break # continue
num_evaluated += 1
if num_remaining < min_remaining:
min_remaining = num_remaining
print(
'Iteration: {} | Best: {} | Backtrack: {} | Deadends: {} | Printed: {} | Element: {} | Index: {} | Time: {:.3f}'
.format(num_evaluated, min_remaining, worst_backtrack,
num_deadends, len(printed), element,
id_from_element[element], elapsed_time(start_time)))
if has_gui():
color_structure(element_bodies, printed, element)
next_printed = printed | {element}
if next_printed in visited:
continue
assert check_connected(ground_nodes, next_printed)
if stiffness and not test_stiffness(extrusion_path,
element_from_id,
next_printed,
checker=checker,
verbose=False):
# Hard dead-end
#num_deadends += 1
stiffness_failures += 1
print('Partial structure is not stiff!')
continue
if revisit:
heapq.heappush(
queue, (visits + 1, priority, printed, element, current_conf))
#condition = frozenset()
#condition = set(retrace_elements(visited, printed, horizon=2))
#condition = printed # horizon=1
condition = next_printed
if not sample_remaining(
condition, next_printed, ee_sample_traj, num=num_ee):
num_deadends += 1
print('An end-effector successor could not be sampled!')
continue
print('Sampling transition')
#command = sample_extrusion(print_gen_fn, ground_nodes, printed, element)
command = next(
iter(full_sample_traj(printed, printed, element, connected=True)),
None)
if command is None:
# Soft dead-end
print('The transition could not be sampled!')
extrusion_failures += 1
continue
print('Sampling successors')
if not sample_remaining(
condition, next_printed, full_sample_traj, num=num_arm):
num_deadends += 1
print('A successor could not be sampled!')
continue
start_conf = end_conf = None
if not ee_only:
start_conf, end_conf = command.start_conf, command.end_conf
if (start_conf is not None) and motions and not lazy:
motion_traj = compute_motion(robot,
obstacles,
element_bodies,
printed,
current_conf,
start_conf,
collisions=collisions,
max_time=max_time -
elapsed_time(start_time))
if motion_traj is None:
transit_failures += 1
continue
command.trajectories.insert(0, motion_traj)
visited[next_printed] = Node(command, printed)
if all_elements <= next_printed:
# TODO: anytime mode
min_remaining = 0
plan = retrace_trajectories(visited, next_printed)
if motions and not lazy:
motion_traj = compute_motion(robot,
obstacles,
element_bodies,
frozenset(),
initial_conf,
plan[0].start_conf,
collisions=collisions,
max_time=max_time -
elapsed_time(start_time))
if motion_traj is None:
plan = None
transit_failures += 1
else:
plan.append(motion_traj)
if motions and lazy:
plan = compute_motions(robot,
obstacles,
element_bodies,
initial_conf,
plan,
collisions=collisions,
max_time=max_time -
elapsed_time(start_time))
break
# if plan is not None:
# break
add_successors(queue,
all_elements,
node_points,
ground_nodes,
priority_fn,
next_printed,
node_points[directed[1]],
end_conf,
partial_orders=partial_orders)
data = {
'num_evaluated': num_evaluated,
'min_remaining': min_remaining,
'max_backtrack': worst_backtrack,
'stiffness_failures': stiffness_failures,
'extrusion_failures': extrusion_failures,
'transit_failures': transit_failures,
'num_deadends': num_deadends,
}
return plan, data
