def score_fn(plan):
assert plan is not None
initial_distance = get_distance(
point_from_pose(initial_pose)[:2], goal_pos2d)
final_pose = world.perception.get_pose(block_name)
final_distance = get_distance(
point_from_pose(final_pose)[:2], goal_pos2d)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print(
'Initial: {:.5f} m | Final: {:.5f} | Rotation: {:.5f} rads'.format(
initial_distance, final_distance, quat_distance))
# TODO: compare orientation to final predicted orientation
# TODO: record simulation time in the event that the controller gets stuck
score = {
'initial_distance': initial_distance,
'final_distance': final_distance,
'rotation': quat_distance,
DYNAMICS: parameters_from_name,
}
#_, args = find_unique(lambda a: a[0] == 'push', plan)
#control = args[-1]
return score
def compute_component_mst(node_points,
ground_nodes,
unprinted,
initial_position=None):
# Weighted A*
start_time = time.time()
point_from_vertex = dict(enumerate(node_points))
vertices = {v for e in unprinted for v in e}
components = get_connected_components(vertices, unprinted)
entry_nodes = set(ground_nodes)
if initial_position is not None:
point_from_vertex[INITIAL_NODE] = initial_position
components.append([INITIAL_NODE])
entry_nodes.add(INITIAL_NODE)
edge_weights = {}
for c1, c2 in combinations(range(len(components)), r=2):
# TODO: directed edges from all points to entry nodes
nodes1 = set(components[c1])
nodes2 = set(components[c2])
if (c1 == [INITIAL_NODE]) or (c2 == [INITIAL_NODE]):
nodes1 &= entry_nodes
nodes2 &= entry_nodes
edge_weights[c1, c2] = min(
get_distance(point_from_vertex[n1], point_from_vertex[n2])
for n1, n2 in product(nodes1, nodes2))
tree = compute_spanning_tree(edge_weights)
weight = sum(edge_weights[e] for e in tree)
print(
'Elements: {} | Components: {} | Tree: {} | Weight: {}: Time: {:.3f} '.
format(len(unprinted), len(components), tree, weight,
elapsed_time(start_time)))
return weight
def sample_tool_ik(robot,
tool_pose,
max_attempts=10,
closest_only=False,
get_all=False,
prev_free_list=[],
**kwargs):
generator = get_ik_generator(robot,
tool_pose,
prev_free_list=prev_free_list,
**kwargs)
ik_joints = get_movable_joints(robot)
for _ in range(max_attempts):
try:
solutions = next(generator)
if closest_only and solutions:
current_conf = get_joint_positions(robot, ik_joints)
solutions = [
min(solutions,
key=lambda conf: get_distance(current_conf, conf))
]
solutions = list(
filter(
lambda conf: not violates_limits(robot, ik_joints, conf),
solutions))
return solutions if get_all else select_solution(
robot, ik_joints, solutions, **kwargs)
except StopIteration:
break
return None
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 compute_sequence_distance(node_points,
directed_elements,
start=None,
end=None):
if directed_elements is None:
return INF
distance = 0.
position = start
for (n1, n2) in directed_elements:
if position is not None:
distance += get_distance(position, node_points[n1])
distance += get_distance(node_points[n1], node_points[n2])
position = node_points[n2]
if end is not None:
distance += get_distance(position, end)
return distance
def greedily_plan(all_elements, node_points, ground_nodes, remaining,
initial_point):
from extrusion.heuristics import compute_z_distance
level_from_node, cost_from_edge = compute_layer_from_directed(
all_elements, node_points, ground_nodes, remaining)
# sequence = sorted(tree_elements, key=lambda e: cost_from_edge[e])
if cost_from_edge is None:
return level_from_node, cost_from_edge, None
point = initial_point
sequence = []
remaining_elements = set(cost_from_edge)
while remaining_elements:
# TODO: sample different random seeds
#key_fn = lambda d: (cost_from_edge[d], random.random())
#key_fn = lambda d: (cost_from_edge[d], compute_z_distance(node_points, d))
key_fn = lambda d: (cost_from_edge[d],
get_distance(point, node_points[d[0]]))
directed = min(remaining_elements, key=key_fn)
remaining_elements.remove(directed)
sequence.append(directed)
point = node_points[directed[1]]
# draw_ordered(sequence, node_points)
# wait_for_user()
return level_from_node, cost_from_edge, sequence
def solve_inverse_kinematics(self,
world_from_tool,
nearby_tolerance=INF,
**kwargs):
if self.ik_solver is not None:
return self.solve_trac_ik(world_from_tool, **kwargs)
#if nearby_tolerance != INF:
# return self.solve_pybullet_ik(world_from_tool, nearby_tolerance=nearby_tolerance)
current_conf = get_joint_positions(self.robot, self.arm_joints)
start_time = time.time()
if nearby_tolerance == INF:
generator = ikfast_inverse_kinematics(self.robot,
PANDA_INFO,
self.tool_link,
world_from_tool,
max_attempts=10,
use_halton=True)
else:
generator = closest_inverse_kinematics(
self.robot,
PANDA_INFO,
self.tool_link,
world_from_tool,
max_time=0.01,
max_distance=nearby_tolerance,
use_halton=True)
conf = next(generator, None)
#conf = sample_tool_ik(self.robot, world_from_tool, max_attempts=100)
if conf is None:
return conf
max_distance = get_distance(current_conf, conf, norm=INF)
#print('Time: {:.3f} | distance: {:.5f} | max: {:.5f}'.format(
# elapsed_time(start_time), max_distance, nearby_tolerance))
set_joint_positions(self.robot, self.arm_joints, conf)
return get_configuration(self.robot)
def score_fn(plan):
assert plan is not None
final_pose = world.get_pose(bowl_name)
point_distance = get_distance(point_from_pose(initial_pose),
point_from_pose(final_pose)) #, norm=2)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format(
point_distance, quat_distance))
with ClientSaver(world.client):
bowl_beads = get_contained_beads(bowl_body, init_beads)
fraction_bowl = float(
len(bowl_beads)) / len(init_beads) if init_beads else 0
mass_in_bowl = sum(map(get_mass, bowl_beads))
spoon_beads = get_contained_beads(world.get_body(spoon_name),
init_beads)
fraction_spoon = float(
len(spoon_beads)) / len(init_beads) if init_beads else 0
mass_in_spoon = sum(map(get_mass, spoon_beads))
print('In Bowl: {:.3f} | In Spoon: {:.3f}'.format(
fraction_bowl, fraction_spoon))
# TODO: measure change in roll/pitch
# TODO: could make latent parameters field
score = {
# Displacements
'bowl_translation': point_distance,
'bowl_rotation': quat_distance,
# Masses
'total_mass': init_mass,
'mass_in_bowl': mass_in_bowl,
'mass_in_spoon': mass_in_spoon,
'spoon_mass_capacity':
(init_mass / len(init_beads)) * spoon_capacity,
# Counts
'num_beads': len(init_beads),
'bowl_beads': len(bowl_beads),
'spoon_beads': len(spoon_beads),
'spoon_capacity': spoon_capacity,
# Fractions
'fraction_in_bowl': fraction_bowl,
'fraction_in_spoon': fraction_spoon,
# Latent
'bead_radius': bead_radius,
DYNAMICS: parameters_from_name
}
fraction_filled = float(score['spoon_beads']) / score['spoon_capacity']
spilled_beads = score['num_beads'] - (score['bowl_beads'] +
score['spoon_beads'])
fraction_spilled = float(spilled_beads) / score['num_beads']
print('Fraction Filled: {} | Fraction Spilled: {}'.format(
fraction_filled, fraction_spilled))
#_, args = find_unique(lambda a: a[0] == 'scoop', plan)
#control = args[-1]
return score
def sample_tool_ik(robot, tool_pose, closest_only=False, get_all=False, **kwargs):
generator = get_ik_generator(robot, tool_pose)
ik_joints = get_movable_joints(robot)
solutions = next(generator)
if closest_only and solutions:
current_conf = get_joint_positions(robot, ik_joints)
solutions = [min(solutions, key=lambda conf: get_distance(current_conf, conf))]
solutions = list(filter(lambda conf: not violates_limits(robot, ik_joints, conf), solutions))
return solutions if get_all else select_solution(robot, ik_joints, solutions, **kwargs)
def control_state(robot,
joints,
target_positions,
target_velocities=None,
position_tol=INF,
velocity_tol=INF,
max_velocities=None,
verbose=True): # TODO: max_accelerations
if target_velocities is None:
target_velocities = np.zeros(len(joints))
if max_velocities is None:
max_velocities = get_max_velocities(robot, joints)
assert (max_velocities > 0).all()
max_velocity_control_joints(robot,
joints,
positions=target_positions,
velocities=target_velocities,
max_velocities=max_velocities)
for i in irange(INF):
current_positions = np.array(get_joint_positions(robot, joints))
position_error = get_distance(current_positions,
target_positions,
norm=INF)
current_velocities = np.array(get_joint_velocities(robot, joints))
velocity_error = get_distance(current_velocities,
target_velocities,
norm=INF)
if verbose:
# print('Positions: {} | Target positions: {}'.format(current_positions.round(N_DIGITS), target_positions.round(N_DIGITS)))
# print('Velocities: {} | Target velocities: {}'.format(current_velocities.round(N_DIGITS), target_velocities.round(N_DIGITS)))
print(
'Step: {} | Position error: {:.3f}/{:.3f} | Velocity error: {:.3f}/{:.3f}'
.format(i, position_error, position_tol, velocity_error,
velocity_tol))
# TODO: draw the tolerance interval
if (position_error <= position_tol) and (velocity_error <=
velocity_tol):
return # TODO: declare success or failure by yielding or throwing an exception
yield i
def compute_distance_from_node(elements, node_points, ground_nodes):
#incoming_supporters, _ = neighbors_from_orders(get_supported_orders(
# element_from_id.values(), node_points))
nodes = nodes_from_elements(elements)
neighbors = adjacent_from_edges(elements)
edge_costs = {edge: get_distance(node_points[edge[0]], node_points[edge[1]])
for edge in elements}
#edge_costs = {edge: np.abs(node_points[edge[1]] - node_points[edge[0]])[2]
# for edge in elements}
edge_costs.update({edge[::-1]: distance for edge, distance in edge_costs.items()})
successor_fn = lambda v: neighbors[v]
cost_fn = lambda v1, v2: edge_costs[v1, v2] # TODO: z on cost function
return dijkstra(ground_nodes & nodes, successor_fn, cost_fn)
def compute_dispersion(bowl_body, beads_per_color):
# homogeneous mixture, dispersion
# Mean absolute difference
# Distance correlation
distances = []
for beads in beads_per_color:
bowl_beads = get_contained_beads(bowl_body, beads)
points = list(map(get_point, bowl_beads))
distances.extend([
get_distance(p1, p2, norm=2)
for p1, p2 in combinations(points, r=2)
])
return np.mean(distances)
def compute_transit_distance(node_points,
directed_elements,
start=None,
end=None):
if directed_elements is None:
return INF
assert directed_elements
# Could instead compute full distance and subtract
pairs = []
if start is not None:
pairs.append((start, node_points[directed_elements[0][0]]))
pairs.extend((node_points[directed1[1]], node_points[directed2[0]])
for directed1, directed2 in get_pairs(directed_elements))
if end is not None:
pairs.append((node_points[directed_elements[-1][1]], end))
return sum(get_distance(*pair) for pair in pairs)
def score_fn(plan):
assert plan is not None
final_pose = world.get_pose(bowl_name)
point_distance = get_distance(point_from_pose(initial_pose),
point_from_pose(final_pose)) #, norm=2)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format(
point_distance, quat_distance))
with ClientSaver(world.client):
# TODO: lift the bowl up (with particles around) to prevent scale detections
final_bowl_beads = get_contained_beads(bowl_body, init_beads)
fraction_bowl = safe_ratio(len(final_bowl_beads),
len(init_beads),
undefined=0)
mass_in_bowl = sum(map(get_mass, final_bowl_beads))
final_cup_beads = get_contained_beads(cup_body, init_beads)
fraction_cup = safe_ratio(len(final_cup_beads),
len(init_beads),
undefined=0)
mass_in_cup = sum(map(get_mass, final_cup_beads))
print('In Bowl: {} | In Cup: {}'.format(fraction_bowl, fraction_cup))
score = {
# Displacements
'bowl_translation': point_distance,
'bowl_rotation': quat_distance,
# Masses
'mass_in_bowl': mass_in_bowl,
'mass_in_cup': mass_in_cup,
# Counts
'bowl_beads': len(final_bowl_beads),
'cup_beads': len(final_cup_beads),
# Fractions
'fraction_in_bowl': fraction_bowl,
'fraction_in_cup': fraction_cup,
}
score.update(latent)
# TODO: store the cup path length to bias towards shorter paths
#_, args = find_unique(lambda a: a[0] == 'pour', plan)
#control = args[-1]
#feature = control['feature']
#parameter = control['parameter']
return score
def solve_pybullet_ik(self, world_from_tool, nearby_tolerance):
start_time = time.time()
# Most of the time is spent creating the robot
# TODO: use the waypoint version that doesn't repeatedly create the robot
current_conf = get_joint_positions(self.robot, self.arm_joints)
full_conf = sub_inverse_kinematics(
self.robot,
self.arm_joints[0],
self.tool_link,
world_from_tool,
custom_limits=self.custom_limits
) # , max_iterations=1) # , **kwargs)
conf = get_joint_positions(self.robot, self.arm_joints)
max_distance = get_distance(current_conf, conf, norm=INF)
if nearby_tolerance < max_distance:
return None
print('Nearby) time: {:.3f} | distance: {:.5f}'.format(
elapsed_time(start_time), max_distance))
return full_conf
def score_fn(plan):
assert plan is not None
with ClientSaver(world.client):
rgb_image = take_image(world, bowl_body, beads_per_color)
values = score_image(rgb_image, bead_colors, beads_per_color)
final_pose = perception.get_pose(bowl_name)
point_distance = get_distance(point_from_pose(initial_pose),
point_from_pose(final_pose)) #, norm=2)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format(
point_distance, quat_distance))
with ClientSaver(world.client):
all_beads = list(flatten(beads_per_color))
bowl_beads = get_contained_beads(bowl_body, all_beads)
fraction_bowl = float(
len(bowl_beads)) / len(all_beads) if all_beads else 0
print('In Bowl: {}'.format(fraction_bowl))
with ClientSaver(world.client):
final_dispersion = compute_dispersion(bowl_body, beads_per_color)
print('Initial Dispersion: {:.3f} | Final Dispersion {:.3f}'.format(
initial_distance, final_dispersion))
score = {
'bowl_translation': point_distance,
'bowl_rotation': quat_distance,
'fraction_in_bowl': fraction_bowl,
'initial_dispersion': initial_distance,
'final_dispersion': final_dispersion,
'num_beads': len(all_beads), # Beads per color
DYNAMICS: parameters_from_name,
}
# TODO: include time required for stirring
# TODO: change in dispersion
#wait_for_user()
#_, args = find_unique(lambda a: a[0] == 'stir', plan)
#control = args[-1]
return score
def score_poses(problem, task, ros_world):
cup_name, bowl_name = REQUIREMENT_FNS[problem](ros_world)
name_from_type = {'cup': cup_name, 'bowl': bowl_name}
initial_poses = {
ty: ros_world.get_pose(name)
for ty, name in name_from_type.items()
}
final_stackings = wait_until_observation(problem, ros_world)
#final_stackings = None
if final_stackings is None:
# TODO: only do this for the bad bowl
point_distances = {ty: INF for ty in name_from_type}
quat_distances = {ty: 2 * np.pi
for ty in name_from_type} # Max rotation
else:
final_poses = {
ty: ros_world.get_pose(name)
for ty, name in name_from_type.items()
}
point_distances = {
ty: get_distance(point_from_pose(initial_poses[ty]),
point_from_pose(final_poses[ty]))
for ty in name_from_type
}
# TODO: wrap around distance for symmetric things
quat_distances = {
ty: quat_angle_between(quat_from_pose(initial_poses[ty]),
quat_from_pose(final_poses[ty]))
for ty in name_from_type
}
score = {}
for ty in sorted(name_from_type):
print(
'{} | translation (m): {:.3f} | rotation (degrees): {:.3f}'.format(
ty, point_distances[ty], math.degrees(quat_distances[ty])))
score.update({
'{}_translation'.format(ty): point_distances[ty],
'{}_rotation'.format(ty): quat_distances[ty],
})
return score
def compute_euclidean_tree(node_points,
ground_nodes,
elements,
initial_position=None):
# remove printed elements from the tree
# TODO: directed version
start_time = time.time()
point_from_vertex, edges = embed_graph(elements, node_points, ground_nodes,
initial_position)
edge_weights = {(n1, n2): get_distance(point_from_vertex[n1],
point_from_vertex[n2])
for n1, n2 in edges}
components = get_connected_components(point_from_vertex, edge_weights)
tree = compute_spanning_tree(edge_weights)
from extrusion.visualization import draw_model
draw_model(tree, point_from_vertex, ground_nodes, color=BLUE)
draw_model(edges - tree, point_from_vertex, ground_nodes, color=RED)
weight = sum(edge_weights[e] for e in tree)
print(len(components), len(point_from_vertex), len(tree), weight,
elapsed_time(start_time))
wait_for_user()
return weight
def optimize_angle(end_effector,
element_pose,
translation,
direction,
reverse,
candidate_angles,
collision_fn,
nearby=True,
max_error=TRANSLATION_TOLERANCE):
robot = end_effector.robot
movable_joints = get_movable_joints(robot)
best_error, best_angle, best_conf = max_error, None, None
initial_conf = get_joint_positions(robot, movable_joints)
for angle in candidate_angles:
grasp_pose = get_grasp_pose(translation, direction, angle, reverse)
# Pose_{world,EE} = Pose_{world,element} * Pose_{element,EE}
# = Pose_{world,element} * (Pose_{EE,element})^{-1}
target_pose = multiply(element_pose, invert(grasp_pose))
set_pose(end_effector.body,
multiply(target_pose, end_effector.tool_from_ee))
if nearby:
set_joint_positions(robot, movable_joints, initial_conf)
conf = solve_ik(end_effector, target_pose, nearby=nearby)
if (conf is None) or collision_fn(conf):
continue
set_joint_positions(robot, movable_joints, conf)
link_pose = get_link_pose(robot, end_effector.tool_link)
error = get_distance(point_from_pose(target_pose),
point_from_pose(link_pose))
if error < best_error: # TODO: error a function of direction as well
best_error, best_angle, best_conf = error, angle, conf
#break
if best_conf is not None:
set_joint_positions(robot, movable_joints, best_conf)
return best_angle, best_conf
def score_image(rgb_image, bead_colors, beads_per_color, max_distance=0.1):
# TODO: could floodfill to identify bead clusters (and reward more clusters)
# TODO: ensure the appropriate ratios are visible on top
# TODO: penalize marbles that have left the bowl
# TODO: estimate the number of escaped marbles using the size at that distance
bead_pixels = [[] for _ in bead_colors]
for r in range(rgb_image.shape[0]): # height
for c in range(rgb_image.shape[1]): # width
pixel = rgb_image[r, c]
assert pixel[3] == 255
rgb = pixel[:3] / 255.
best_index, best_distance = None, INF
for index, color in enumerate(bead_colors):
distance = np.linalg.norm(rgb - color[:3])
if distance < best_distance:
best_index, best_distance = index, distance
if best_distance <= max_distance:
bead_pixels[best_index].append((r, c))
# TODO: discount beads outside
all_beads = list(flatten(beads_per_color))
bead_frequencies = np.array([len(beads) for beads in beads_per_color],
dtype=float) / len(all_beads)
all_pixels = list(flatten(bead_pixels))
image_frequencies = np.array([len(beads) for beads in bead_pixels],
dtype=float) / len(all_pixels)
print(bead_frequencies, image_frequencies,
image_frequencies - bead_frequencies)
distances = []
for pixels in bead_pixels:
distances.extend([
get_distance(p1, p2, norm=2)
for p1, p2 in combinations(pixels, r=2)
])
dispersion = np.mean(distances) # TODO: translate into meters?
print(dispersion)
def retime_waypoints(waypoints, start_time=0.):
durations = [start_time] + [
get_distance(*pair) / TOOL_VELOCITY for pair in get_pairs(waypoints)
]
return np.cumsum(durations)
def fn(*locations):
return 1. + get_distance(*map(extract_point, locations))
def solve_tsp(all_elements,
ground_nodes,
node_points,
printed,
initial_point,
final_point,
bidirectional=False,
layers=True,
max_time=30,
visualize=True,
verbose=False):
# https://developers.google.com/optimization/routing/tsp
# https://developers.google.com/optimization/reference/constraint_solver/routing/RoutingModel
# http://www.math.uwaterloo.ca/tsp/concorde.html
# https://developers.google.com/optimization/reference/python/constraint_solver/pywrapcp
# AddDisjunction
# TODO: pick up and delivery
# TODO: time window for skipping elements
# TODO: Minimum Spanning Tree (MST) bias
# TODO: time window constraint to ensure connected
# TODO: reuse by simply swapping out the first vertex
# arc-routing
from ortools.constraint_solver import routing_enums_pb2, pywrapcp
from extrusion.visualization import draw_ordered, draw_model
start_time = time.time()
assert initial_point is not None
remaining = all_elements - printed
#printed_nodes = compute_printed_nodes(ground_nodes, printed)
if not remaining:
cost = get_distance(initial_point, final_point)
return [], cost
# TODO: use as a lower bound
total_distance = compute_element_distance(node_points, remaining)
# TODO: some of these are invalid still
level_from_node, cost_from_edge, sequence = greedily_plan(
all_elements, node_points, ground_nodes, remaining, initial_point)
if sequence is None:
return None, INF
extrusion_edges = set()
point_from_vertex = {INITIAL_NODE: initial_point, FINAL_NODE: final_point}
frame_nodes = nodes_from_elements(remaining)
min_level = min(level_from_node[n] for n in frame_nodes)
max_level = max(level_from_node[n] for n in frame_nodes)
frame_keys = set()
keys_from_node = defaultdict(set)
for element in remaining:
mid = (element, element)
point_from_vertex[mid] = get_midpoint(node_points, element)
for node in element:
key = (element, node)
point_from_vertex[key] = node_points[node]
frame_keys.add(key)
keys_from_node[node].add(key)
for reverse in [True, False]:
directed = reverse_element(element) if reverse else element
node1, node2 = directed
#delta = node_points[node2] - node_points[node1]
#pitch = get_pitch(delta)
#upward = -SUPPORT_THETA <= pitch
# theta = angle_between(delta, [0, 0, -1])
# upward = theta < (np.pi / 2 - SUPPORT_THETA)
#if (directed in tree_elements): # or upward:
if bidirectional or (directed in cost_from_edge):
# Add edges from anything that is roughly the correct cost
start = (element, node1)
end = (element, node2)
extrusion_edges.update({(start, mid), (mid, end)})
for node in keys_from_node:
for edge in product(keys_from_node[node], repeat=2):
extrusion_edges.add(edge)
# Key thing is partial order on buckets of elements to adhere to height
# Connect v2 to v1 if v2 is the same level
# Traversing an edge might move to a prior level (but at most one)
transit_edges = set()
for directed in product(frame_keys, repeat=2):
key1, key2 = directed
element1, node1 = key1
#level1 = min(level_from_node[n] for n in element1)
level1 = level_from_node[get_other_node(node1, element1)]
_, node2 = key2
level2 = level_from_node[node2]
if level2 in [level1, level1 + 1]: # TODO: could bucket more coarsely
transit_edges.add(directed)
for key in frame_keys:
_, node = key
if level_from_node[node] == min_level:
transit_edges.add((INITIAL_NODE, key))
if level_from_node[node] in [max_level, max_level - 1]:
transit_edges.add((key, FINAL_NODE))
# TODO: can also remove restriction that elements are printed in a single direction
if not layers:
transit_edges.update(
product(frame_keys | {INITIAL_NODE, FINAL_NODE},
repeat=2)) # TODO: apply to greedy as well
key_from_index = list(
{k
for pair in extrusion_edges | transit_edges for k in pair})
edge_weights = {
pair: INVALID
for pair in product(key_from_index, repeat=2)
}
for k1, k2 in transit_edges | extrusion_edges:
p1, p2 = point_from_vertex[k1], point_from_vertex[k2]
edge_weights[k1, k2] = get_distance(p1, p2)
#edge_weights.update({e: 0. for e in extrusion_edges}) # frame edges are free
edge_weights[FINAL_NODE, INITIAL_NODE] = 0.
edge_weights[INITIAL_NODE,
FINAL_NODE] = INVALID # Otherwise might be backward
print(
'Elements: {} | Vertices: {} | Edges: {} | Structure: {:.3f} | Min Level {} | Max Level: {}'
.format(len(remaining), len(key_from_index), len(edge_weights),
total_distance, min_level, max_level))
index_from_key = dict(map(reversed, enumerate(key_from_index)))
num_vehicles, depot = 1, index_from_key[INITIAL_NODE]
manager = pywrapcp.RoutingIndexManager(len(key_from_index), num_vehicles,
depot)
#[depot], [depot])
solver = pywrapcp.RoutingModel(manager)
cost_from_index = {}
for (k1, k2), weight in edge_weights.items():
i1, i2 = index_from_key[k1], index_from_key[k2]
cost_from_index[i1, i2] = int(math.ceil(SCALE * weight))
solver.SetArcCostEvaluatorOfAllVehicles(
solver.RegisterTransitCallback(
lambda i1, i2: cost_from_index[manager.IndexToNode(
i1), manager.IndexToNode(i2)])) # from -> to
# sequence = plan_stiffness(None, None, node_points, ground_nodes, elements,
# initial_position=initial_point, stiffness=False, max_backtrack=INF)
initial_order = []
#initial_order = [INITIAL_NODE] # Start and end automatically included
for directed in sequence:
node1, node2 = directed
element = get_undirected(remaining, directed)
initial_order.extend([
(element, node1),
(element, element),
(element, node2),
])
initial_order.append(FINAL_NODE)
#initial_order.append(INITIAL_NODE)
initial_route = [index_from_key[key] for key in initial_order]
#index = initial_route.index(0)
#initial_route = initial_route[index:] + initial_route[:index] + [0]
initial_solution = solver.ReadAssignmentFromRoutes(
[initial_route], ignore_inactive_indices=True)
assert initial_solution is not None
#print_solution(manager, solver, initial_solution)
#print(solver.GetAllDimensionNames())
#print(solver.ComputeLowerBound())
objective = initial_solution.ObjectiveValue() / SCALE
invalid = int(objective / INVALID)
order = parse_solution(solver, manager, key_from_index,
initial_solution)[:-1]
ordered_pairs = get_pairs(order)
cost = sum(edge_weights[pair] for pair in ordered_pairs)
#print('Initial solution | Invalid: {} | Objective: {:.3f} | Cost: {:.3f} | Duration: {:.3f}s'.format(
# invalid, objective, cost, elapsed_time(start_time)))
if False and visualize: # and invalid
remove_all_debug()
draw_model(printed, node_points, None, color=BLACK)
draw_point(initial_point, color=BLACK)
draw_point(final_point, color=GREEN)
for pair in ordered_pairs:
if edge_weights[pair] == INVALID:
for key in pair:
draw_point(point_from_vertex[key], color=RED)
draw_ordered(ordered_pairs, point_from_vertex)
wait_for_user() # TODO: pause only if viewer
#sequence = extract_sequence(level_from_node, remaining, ordered_pairs)
#print(compute_sequence_distance(node_points, sequence, start=initial_point, end=final_point), total_distance+cost)
#print([cost_from_edge[edge] for edge in sequence])
#return sequence, cost
start_time = time.time()
search_parameters = pywrapcp.DefaultRoutingSearchParameters()
#search_parameters.solution_limit = 1
search_parameters.time_limit.seconds = int(max_time)
search_parameters.log_search = verbose
# AUTOMATIC | PATH_CHEAPEST_ARC | LOCAL_CHEAPEST_ARC | GLOBAL_CHEAPEST_ARC | LOCAL_CHEAPEST_INSERTION
#search_parameters.first_solution_strategy = (
# routing_enums_pb2.FirstSolutionStrategy.AUTOMATIC)
# AUTOMATIC | GREEDY_DESCENT | GUIDED_LOCAL_SEARCH | SIMULATED_ANNEALING | TABU_SEARCH | OBJECTIVE_TABU_SEARCH
#search_parameters.local_search_metaheuristic = (
# routing_enums_pb2.LocalSearchMetaheuristic.GREEDY_DESCENT)
#solution = solver.SolveWithParameters(search_parameters)
solution = solver.SolveFromAssignmentWithParameters(
initial_solution, search_parameters)
#print_solution(manager, solver, solution)
print('Status: {} | Text: {}'.format(solver.status(),
STATUS[solver.status()]))
if not solution:
print('Failure! Duration: {:.3f}s'.format(elapsed_time(start_time)))
return None, INF
objective = solution.ObjectiveValue() / SCALE
invalid = int(objective / INVALID)
order = parse_solution(solver, manager, key_from_index, solution)[:-1]
ordered_pairs = get_pairs(order) # + [(order[-1], order[0])]
#cost = compute_element_distance(point_from_vertex, ordered_pairs)
cost = sum(edge_weights[pair] for pair in ordered_pairs)
print(
'Final solution | Invalid: {} | Objective: {:.3f} | Cost: {:.3f} | Duration: {:.3f}s'
.format(invalid, objective, cost, elapsed_time(start_time)))
sequence = extract_sequence(level_from_node, remaining, ordered_pairs)
#print(compute_sequence_distance(node_points, sequence, start=initial_point, end=final_point)) #, total_distance+cost)
#print(sequence)
violations = 0
printed_nodes = compute_printed_nodes(ground_nodes, printed)
for directed in sequence:
if directed[0] not in printed_nodes:
#print(directed)
violations += 1
printed_nodes.update(directed)
print('Violations:', violations)
if visualize:
# TODO: visualize by weight
remove_all_debug()
draw_model(printed, node_points, None, color=BLACK)
draw_point(initial_point, color=BLACK)
draw_point(final_point, color=GREEN)
#tour_pairs = ordered_pairs + get_pairs(list(reversed(order)))
#draw_model(extrusion_edges - set(tour_pairs), point_from_vertex, ground_nodes, color=BLACK)
draw_ordered(ordered_pairs, point_from_vertex)
wait_for_user()
if sequence is None:
return None, INF
#print([cost_from_edge[edge] for edge in sequence])
return sequence, cost
def fn(printed, directed, position, conf):
# Queue minimizes the statistic
element = get_undirected(all_elements, directed)
n1, n2 = directed
# forward adds, backward removes
structure = printed | {element} if forward else printed - {element}
structure_ids = get_extructed_ids(element_from_id, structure)
normalizer = 1
#normalizer = len(structure)
#normalizer = compute_element_distance(node_points, all_elements)
reduce_op = sum # sum | max | average
reaction_fn = force_from_reaction # force_from_reaction | torque_from_reaction
first_node, second_node = directed if forward else reverse_element(directed)
layer = sign * layer_from_edge[element]
#layer = sign * layer_from_directed.get(directed, INF)
tool_point = position
tool_distance = 0.
if heuristic in COST_HEURISTICS:
if conf is not None:
if hash_or_id(conf) not in ee_cache:
with BodySaver(robot):
set_configuration(robot, conf)
ee_cache[hash_or_id(conf)] = get_tool_position(robot)
tool_point = ee_cache[hash_or_id(conf)]
tool_distance = get_distance(tool_point, node_points[first_node])
# TODO: weighted average to balance cost and bias
if heuristic == 'none':
return 0
if heuristic == 'random':
return random.random()
elif heuristic == 'degree':
# TODO: other graph statistics
#printed_nodes = {n for e in printed for n in e}
#node = n1 if n2 in printed_nodes else n2
#if node in ground_nodes:
# return 0
raise NotImplementedError()
elif heuristic == 'length':
# Equivalent to mass if uniform density
return get_element_length(element, node_points)
elif heuristic == 'distance':
return tool_distance
elif heuristic == 'layered-distance':
return (layer, tool_distance)
# elif heuristic == 'components':
# # Ground nodes intentionally omitted
# # TODO: this is broken
# remaining = all_elements - printed if forward else printed - {element}
# vertices = nodes_from_elements(remaining)
# components = get_connected_components(vertices, remaining)
# #print('Components: {} | Distance: {:.3f}'.format(len(components), tool_distance))
# return (len(components), tool_distance)
elif heuristic == 'fixed-tsp':
# TODO: layer_from_edge[element]
# TODO: score based on current distance from the plan in the tour
# TODO: factor in the distance to the next element in a more effective way
if order is None:
return (INF, tool_distance)
return (sign*order[element], tool_distance) # Chooses least expensive direction
elif heuristic == 'tsp':
if printed not in tsp_cache: # not last_plan and
# TODO: seed with the previous solution
#remaining = all_elements - printed if forward else printed
#assert element in remaining
#printed_nodes = compute_printed_nodes(ground_nodes, printed) if forward else ground_nodes
tsp_cache[printed] = solve_tsp(all_elements, ground_nodes, node_points, printed, tool_point, initial_point,
bidirectional=True, layers=True, max_time=30, visualize=False, verbose=True)
#print(tsp_cache[printed])
if not last_plan:
last_plan[:] = tsp_cache[printed][0]
plan, cost = tsp_cache[printed]
#plan = last_plan[len(printed):]
if plan is None:
#return tool_distance
return (layer, INF)
transit = compute_transit_distance(node_points, plan, start=tool_point, end=initial_point)
assert forward
first = plan[0] == directed
#return not first # No info if the best isn't possible
index = None
for i, directed2 in enumerate(plan):
undirected2 = get_undirected(all_elements, directed2)
if element == undirected2:
assert index is None
index = i
assert index is not None
# Could also break ties by other in the plan
# Two plans might have the same cost but the swap might be detrimental
new_plan = [directed] + plan[:index] + plan[index+1:]
assert len(plan) == len(new_plan)
new_transit = compute_transit_distance(node_points, new_plan, start=tool_point, end=initial_point)
#print(layer, cost, transit + compute_element_distance(node_points, plan),
# new_transit + compute_element_distance(node_points, plan))
#return new_transit
return (layer, not first, new_transit) # Layer important otherwise it shortcuts
elif heuristic == 'online-tsp':
if forward:
_, tsp_distance = solve_tsp(all_elements-structure, ground_nodes, node_points, printed,
node_points[second_node], initial_point, visualize=False)
else:
_, tsp_distance = solve_tsp(structure, ground_nodes, node_points, printed, initial_point,
node_points[second_node], visualize=False)
total = tool_distance + tsp_distance
return total
# elif heuristic == 'mst':
# # TODO: this is broken
# mst_distance = compute_component_mst(node_points, ground_nodes, remaining,
# initial_position=node_points[second_node])
# return tool_distance + mst_distance
elif heuristic == 'x':
return sign * get_midpoint(node_points, element)[0]
elif heuristic == 'z':
return sign * compute_z_distance(node_points, element)
elif heuristic == 'pitch':
#delta = node_points[second_node] - node_points[first_node]
delta = node_points[n2] - node_points[n1]
return get_pitch(delta)
elif heuristic == 'dijkstra': # offline
# TODO: sum of all element path distances
return sign*np.average([distance_from_node[node].cost for node in element]) # min, max, average
elif heuristic == 'online-dijkstra':
if printed not in distance_cache:
distance_cache[printed] = compute_distance_from_node(printed, node_points, ground_nodes)
return sign*min(distance_cache[printed][node].cost
if node in distance_cache[printed] else INF
for node in element)
elif heuristic == 'plan-stiffness':
if order is None:
return None
return (sign*order[element], directed not in order)
elif heuristic == 'load':
nodal_loads = checker.get_nodal_loads(existing_ids=structure_ids, dof_flattened=False) # get_self_weight_loads
return reduce_op(np.linalg.norm(force_from_reaction(reaction)) for reaction in nodal_loads.values())
elif heuristic == 'fixed-forces':
#printed = all_elements # disable to use most up-to-date
# TODO: relative to the load introduced
if printed not in reaction_cache:
reaction_cache[printed] = compute_all_reactions(extrusion_path, all_elements, checker=checker)
force = reduce_op(np.linalg.norm(reaction_fn(reaction)) for reaction in reaction_cache[printed].reactions[element])
return force / normalizer
elif heuristic == 'forces':
reactions_from_nodes = compute_node_reactions(extrusion_path, structure, checker=checker)
#torque = sum(np.linalg.norm(np.sum([torque_from_reaction(reaction) for reaction in reactions], axis=0))
# for reactions in reactions_from_nodes.values())
#return torque / normalizer
total = reduce_op(np.linalg.norm(reaction_fn(reaction)) for reactions in reactions_from_nodes.values()
for reaction in reactions)
return total / normalizer
#return max(sum(np.linalg.norm(reaction_fn(reaction)) for reaction in reactions)
# for reactions in reactions_from_nodes.values())
elif heuristic == 'stiffness':
# TODO: add different variations
# TODO: normalize by initial stiffness, length, or degree
# Most unstable or least unstable first
# Gets faster with fewer all_elements
#old_stiffness = score_stiffness(extrusion_path, element_from_id, printed, checker=checker)
stiffness = score_stiffness(extrusion_path, element_from_id, structure, checker=checker) # lower is better
return stiffness / normalizer
#return stiffness / old_stiffness
elif heuristic == 'fixed-stiffness':
# TODO: invert the sign for regression/progression?
# TODO: sort FastDownward by the (fixed) action cost
return stiffness_cache[element] / normalizer
elif heuristic == 'relative-stiffness':
stiffness = score_stiffness(extrusion_path, element_from_id, structure, checker=checker) # lower is better
if normalizer == 0:
return 0
return stiffness / normalizer
#return stiffness / stiffness_cache[element]
raise ValueError(heuristic)
def get_element_length(element, node_points):
n1, n2 = element
return get_distance(node_points[n2], node_points[n1])
def trajectory_cost_fn(t):
distance = t.distance(
distance_fn=lambda q1, q2: get_distance(q1[:2], q2[:2]))
return BASE_CONSTANT + distance / BASE_VELOCITY
def get_link_distance(self, **kwargs):
# TODO: just endpoints
link_path = list(map(point_from_pose, self.get_link_path(**kwargs)))
return sum(get_distance(*pair) for pair in get_pairs(link_path))
def base_cost_fn(q1, q2):
distance = get_distance(q1.values[:2], q2.values[:2])
return BASE_CONSTANT + distance / BASE_VELOCITY
