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