def __init__(self,
end_effector,
element_bodies,
node_points,
ground_nodes,
element,
reverse,
max_directions=INF):
# TODO: more directions for ground collisions
self.end_effector = end_effector
self.element = element
self.reverse = reverse
self.max_directions = max_directions
self.element_pose = get_pose(element_bodies[element])
n1, n2 = element
length = get_distance(node_points[n1], node_points[n2])
self.translation_path = np.append(
np.arange(-length / 2, length / 2, STEP_SIZE), [length / 2])
if ORTHOGONAL_GROUND and is_ground(element, ground_nodes):
# TODO: orthogonal to the ground
self.direction_generator = cycle(
[Pose(euler=Euler(roll=0, pitch=0))])
else:
self.direction_generator = get_direction_generator(
self.end_effector, use_halton=False)
self.trajectories = []
def draw_model(elements, node_points, ground_nodes, color=None):
handles = []
with LockRenderer():
for element in elements:
if color is None:
element_color = BLUE if is_ground(element,
ground_nodes) else RED
else:
element_color = color
handles.append(
draw_element(node_points, element, color=element_color))
return handles
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))
def display_trajectories(node_points,
ground_nodes,
trajectories,
animate=True,
time_step=0.02,
video=False):
if trajectories is None:
return
set_extrusion_camera(node_points)
planned_elements = recover_sequence(trajectories)
colors = sample_colors(len(planned_elements))
# if not animate:
# draw_ordered(planned_elements, node_points)
# wait_for_user()
# disconnect()
# return
video_saver = None
if video:
handles = draw_model(planned_elements, node_points,
ground_nodes) # Allows user to adjust the camera
wait_for_user()
remove_all_debug()
wait_for_duration(0.1)
video_saver = VideoSaver('video.mp4') # has_gui()
time_step = 0.001
else:
wait_for_user()
#element_bodies = dict(zip(planned_elements, create_elements(node_points, planned_elements)))
#for body in element_bodies.values():
# set_color(body, (1, 0, 0, 0))
connected_nodes = set(ground_nodes)
printed_elements = []
print('Trajectories:', len(trajectories))
for i, trajectory in enumerate(trajectories):
#wait_for_user()
#set_color(element_bodies[element], (1, 0, 0, 1))
last_point = None
handles = []
for _ in trajectory.iterate():
# TODO: the robot body could be different
if isinstance(trajectory, PrintTrajectory):
current_point = point_from_pose(
trajectory.end_effector.get_tool_pose())
if last_point is not None:
# color = BLUE if is_ground(trajectory.element, ground_nodes) else RED
color = colors[len(printed_elements)]
handles.append(
add_line(last_point,
current_point,
color=color,
width=LINE_WIDTH))
last_point = current_point
if time_step is None:
wait_for_user()
else:
wait_for_duration(time_step)
if isinstance(trajectory, PrintTrajectory):
if not trajectory.path:
color = colors[len(printed_elements)]
handles.append(
draw_element(node_points, trajectory.element, color=color))
#wait_for_user()
is_connected = (trajectory.n1 in connected_nodes
) # and (trajectory.n2 in connected_nodes)
print('{}) {:9} | Connected: {} | Ground: {} | Length: {}'.format(
i, str(trajectory), is_connected,
is_ground(trajectory.element, ground_nodes),
len(trajectory.path)))
if not is_connected:
wait_for_user()
connected_nodes.add(trajectory.n2)
printed_elements.append(trajectory.element)
if video_saver is not None:
video_saver.restore()
wait_for_user()
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