def instantaneous_retime_path(robot, joints, path, speed=ARM_SPEED):
#duration_fn = get_distance_fn(robot, joints)
duration_fn = get_duration_fn(robot, joints)
mid_durations = [duration_fn(*pair) for pair in get_pairs(path)]
durations = [0.] + mid_durations
time_from_starts = np.cumsum(durations) / speed
return time_from_starts
def embed_graph(elements, node_points, initial_position=None, num=1):
point_from_vertex = dict(enumerate(node_points))
if initial_position is not None:
point_from_vertex[INITIAL_NODE] = initial_position
frame_edges = set()
for element in elements:
n1, n2 = element
path = [n1]
for t in np.linspace(0, 1, num=2 + num, endpoint=True)[1:-1]:
n3 = len(point_from_vertex)
point_from_vertex[n3] = t * node_points[n1] + (1 -
t) * node_points[n2]
path.append(n3)
path.append(n2)
for directed in get_pairs(path) + get_pairs(list(reversed(path))):
frame_edges.add(directed)
return point_from_vertex, frame_edges
def slow_trajectory(robot,
joints,
path,
min_fraction=0.1,
ramp_duration=1.0,
**kwargs):
"""
:param robot:
:param joints:
:param path:
:param min_fraction: percentage
:param ramp_duration: seconds
:param kwargs:
:return:
"""
time_from_starts = instantaneous_retime_path(robot, joints, path, **kwargs)
mid_times = [np.average(pair) for pair in get_pairs(time_from_starts)]
mid_durations = [t2 - t1 for t1, t2 in get_pairs(time_from_starts)]
new_time_from_starts = [0.]
for mid_time, mid_duration in zip(mid_times, mid_durations):
time_from_start = mid_time - time_from_starts[0]
up_fraction = clip(time_from_start / ramp_duration,
min_value=min_fraction,
max_value=1.)
time_from_end = time_from_starts[-1] - mid_time
down_fraction = clip(time_from_end / ramp_duration,
min_value=min_fraction,
max_value=1.)
new_fraction = min(up_fraction, down_fraction)
new_duration = mid_duration / new_fraction
#print(new_time_from_starts[-1], up_fraction, down_fraction, new_duration)
new_time_from_starts.append(new_time_from_starts[-1] + new_duration)
# print(time_from_starts)
# print(new_time_from_starts)
# raw_input('Continue?)
# time_from_starts = new_time_from_starts
return new_time_from_starts
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 decompose_into_paths(joints, path):
current_path = []
joint_sequence = []
path_sequence = []
for q1, q2 in get_pairs(path):
# Zero velocities aren't enforced otherwise
indices, = np.nonzero(get_difference(q1, q2))
current_joints = tuple(joints[j] for j in indices)
if not joint_sequence or (current_joints != joint_sequence[-1]):
if current_path:
path_sequence.append(current_path)
joint_sequence.append(current_joints)
current_path = [tuple(q1[j] for j in indices)]
current_path.append(tuple(q2[j] for j in indices))
if current_path:
path_sequence.append(current_path)
return zip(joint_sequence, path_sequence)
def ramp_retime_path(path,
max_velocities,
acceleration_fraction=1.5,
sample_step=None):
assert np.all(max_velocities)
accelerations = max_velocities * acceleration_fraction
dim = len(max_velocities)
#difference_fn = get_difference_fn(robot, joints)
# TODO: more fine grain when moving longer distances
# Assuming instant changes in accelerations
waypoints = [path[0]]
time_from_starts = [0.]
for q1, q2 in get_pairs(path):
differences = get_difference(q1, q2) # assumes not circular anymore
#differences = difference_fn(q1, q2)
distances = np.abs(differences)
duration = 0
for idx in range(dim):
total_time = compute_min_duration(distances[idx],
max_velocities[idx],
accelerations[idx])
duration = max(duration, total_time)
time_from_start = time_from_starts[-1]
if sample_step is not None:
ramp_durations = [
compute_ramp_duration(distances[idx], max_velocities[idx],
accelerations[idx], duration)
for idx in range(dim)
]
directions = np.sign(differences)
for t in np.arange(sample_step, duration, sample_step):
positions = []
for idx in range(dim):
distance = compute_position(ramp_durations[idx], duration,
accelerations[idx], t)
positions.append(q1[idx] + directions[idx] * distance)
waypoints.append(positions)
time_from_starts.append(time_from_start + t)
waypoints.append(q2)
time_from_starts.append(time_from_start + duration)
return waypoints, time_from_starts
def draw_last_roadmap(robot,
joints,
only_checked=False,
linear=True,
down_sample=None,
**kwargs):
q0 = get_joint_positions(robot, joints)
handles = []
if not ROADMAPS:
return handles
roadmap = ROADMAPS[-1]
for q in roadmap.samples:
q = q if len(q) == 3 else np.append(q[:2],
q0[2:]) # TODO: make a function
handles.extend(draw_pose2d(q, z=DRAW_Z))
for v1, v2 in roadmap.edges:
color = BLACK
if roadmap.is_colliding(v1, v2):
color = RED
elif roadmap.is_safe(v1, v2):
color = GREEN
elif only_checked:
continue
if linear:
path = [roadmap.samples[v1], roadmap.samples[v2]]
else:
path = roadmap.get_path(v1, v2)
if down_sample is not None:
path = path[::down_sample] + [path[-1]]
#handles.extend(draw_path(path, **kwargs))
points = list(
map(point_from_pose, [
pose_from_pose2d(
q if len(q) == 3 else np.append(q[:2], q0[2:]), z=DRAW_Z)
for q in path
]))
handles.extend(
add_line(p1, p2, color=color) for p1, p2 in get_pairs(points))
return handles
def ramp_retime_path(path,
max_velocities,
acceleration_fraction=INF,
sample_step=None):
"""
:param path:
:param max_velocities:
:param acceleration_fraction: fraction of velocity_fraction*max_velocity per second
:param sample_step:
:return:
"""
assert np.all(max_velocities)
accelerations = max_velocities * acceleration_fraction
dim = len(max_velocities)
#difference_fn = get_difference_fn(robot, joints)
# TODO: more fine grain when moving longer distances
# Assuming instant changes in accelerations
waypoints = [path[0]]
time_from_starts = [0.]
for q1, q2 in get_pairs(path):
differences = get_difference(q1, q2) # assumes not circular anymore
#differences = difference_fn(q1, q2)
distances = np.abs(differences)
duration = max([
compute_min_duration(distances[idx], max_velocities[idx],
accelerations[idx]) for idx in range(dim)
] + [0.])
time_from_start = time_from_starts[-1]
if sample_step is not None:
waypoints, time_from_starts = add_ramp_waypoints(
differences, accelerations, q1, duration, sample_step,
waypoints, time_from_starts)
waypoints.append(q2)
time_from_starts.append(time_from_start + duration)
return waypoints, time_from_starts
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-c', '--cfree', action='store_true',
help='When enabled, disables collision checking.')
# parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name),
# help='The name of the problem to solve.')
parser.add_argument('--holonomic', action='store_true', # '-h',
help='')
parser.add_argument('-l', '--lock', action='store_false',
help='')
parser.add_argument('-s', '--seed', default=None, type=int,
help='The random seed to use.')
parser.add_argument('-v', '--viewer', action='store_false',
help='')
args = parser.parse_args()
connect(use_gui=args.viewer)
seed = args.seed
#seed = 0
#seed = time.time()
set_random_seed(seed=seed) # None: 2147483648 = 2**31
set_numpy_seed(seed=seed)
print('Random seed:', get_random_seed(), random.random())
print('Numpy seed:', get_numpy_seed(), np.random.random())
#########################
robot, base_limits, goal_conf, obstacles = problem1()
draw_base_limits(base_limits)
custom_limits = create_custom_base_limits(robot, base_limits)
base_joints = joints_from_names(robot, BASE_JOINTS)
base_link = link_from_name(robot, BASE_LINK_NAME)
if args.cfree:
obstacles = []
# for obstacle in obstacles:
# draw_aabb(get_aabb(obstacle)) # Updates automatically
resolutions = None
#resolutions = np.array([0.05, 0.05, math.radians(10)])
set_all_static() # Doesn't seem to affect
region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3))
draw_aabb(region_aabb)
step_simulation() # Need to call before get_bodies_in_region
#update_scene() # TODO: https://github.com/bulletphysics/bullet3/pull/3331
bodies = get_bodies_in_region(region_aabb)
print(len(bodies), bodies)
# https://github.com/bulletphysics/bullet3/search?q=broadphase
# https://github.com/bulletphysics/bullet3/search?p=1&q=getCachedOverlappingObjects&type=&utf8=%E2%9C%93
# https://andysomogyi.github.io/mechanica/bullet.html
# http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual
#draw_pose(get_link_pose(robot, base_link), length=0.5)
for conf in [get_joint_positions(robot, base_joints), goal_conf]:
draw_pose(pose_from_pose2d(conf, z=DRAW_Z), length=DRAW_LENGTH)
aabb = get_aabb(robot)
#aabb = get_subtree_aabb(robot, base_link)
draw_aabb(aabb)
for link in [BASE_LINK, base_link]:
print(link, get_collision_data(robot, link), pairwise_link_collision(robot, link, robot, link))
#########################
saver = WorldSaver()
start_time = time.time()
profiler = Profiler(field='tottime', num=50) # tottime | cumtime | None
profiler.save()
with LockRenderer(lock=args.lock):
path = plan_motion(robot, base_joints, goal_conf, holonomic=args.holonomic, obstacles=obstacles,
custom_limits=custom_limits, resolutions=resolutions,
use_aabb=True, cache=True, max_distance=0.,
restarts=2, iterations=20, smooth=20) # 20 | None
saver.restore()
#wait_for_duration(duration=1e-3)
profiler.restore()
#########################
solved = path is not None
length = INF if path is None else len(path)
cost = sum(get_distance_fn(robot, base_joints, weights=resolutions)(*pair) for pair in get_pairs(path))
print('Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format(
solved, length, cost, elapsed_time(start_time)))
if path is None:
disconnect()
return
iterate_path(robot, base_joints, path)
disconnect()
def compute_cost(robot, joints, path, resolutions=None):
if path is None:
return INF
distance_fn = get_distance_fn(robot, joints,
weights=resolutions) # TODO: get_duration_fn
return sum(distance_fn(*pair) for pair in get_pairs(path))
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 get_distance(self):
if not self.path:
return 0.
return sum(
get_cspace_distance(self.robot, *pair)
for pair in get_pairs(self.path))
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 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
