TSP

sgptools.utils.tsp

resample_path(waypoints, num_inducing=10)

Function to map path with arbitrary number of waypoints to inducing points path with fixed number of waypoints

Parameters:

Name	Type	Description	Default
waypoints	ndarray	(num_waypoints, ndim); waypoints of path from vrp solver	required
num_inducing	int	Number of inducing points (waypoints) in the returned path	10

Returns:

Name	Type	Description
points	ndarray	(num_inducing, ndim); Resampled path

Source code in sgptools/utils/tsp.py

def resample_path(waypoints, num_inducing=10):
    """Function to map path with arbitrary number of waypoints to 
    inducing points path with fixed number of waypoints

    Args:
        waypoints (ndarray): (num_waypoints, ndim); waypoints of path from vrp solver
        num_inducing (int): Number of inducing points (waypoints) in the returned path

    Returns:
        points (ndarray): (num_inducing, ndim); Resampled path
    """
    ndim = np.shape(waypoints)[-1]
    if not (ndim==2 or ndim==3):
        raise Exception(f"ndim={ndim} is not supported for path resampling!")
    line = LineString(waypoints)
    distances = np.linspace(0, line.length, num_inducing)
    points = [line.interpolate(distance) for distance in distances]
    if ndim==2:
        points = np.array([[p.x, p.y] for p in points])
    elif ndim==3:
        points = np.array([[p.x, p.y, p.z] for p in points])
    return points

run_tsp(nodes, num_vehicles=1, max_dist=25, depth=1, resample=None, start_nodes=None, end_nodes=None, time_limit=10)

Method to run TSP/VRP with arbitrary start and end nodes, and without any distance constraint

Parameters:

Name	Type	Description	Default
nodes	ndarray	(# nodes, ndim); Nodes to visit	required
num_vehicles	int	Number of robots/vehicles	1
max_dist	float	Maximum distance allowed for each path when handling mutli-robot case	25
depth	int	Internal parameter used to track re-try recursion depth	1
resample	int	Each solution path will be resampled to have resample number of points	None
start_nodes	ndarray	(# num_vehicles, ndim); Optionl array of start nodes from which to start each vehicle's solution path	None
end_nodes	ndarray	(# num_vehicles, ndim); Optionl array of end nodes at which to end each vehicle's solution path	None
time_limit	int	TSP runtime time limit in seconds	10

Returns:

Name	Type	Description
paths	ndarray	Solution paths
distances	list	List of path lengths

Source code in sgptools/utils/tsp.py

def run_tsp(nodes, 
            num_vehicles=1, 
            max_dist=25, 
            depth=1, 
            resample=None, 
            start_nodes=None,
            end_nodes=None,
            time_limit=10):
    """Method to run TSP/VRP with arbitrary start and end nodes, 
    and without any distance constraint

    Args:
        nodes (ndarray): (# nodes, ndim); Nodes to visit 
        num_vehicles (int): Number of robots/vehicles
        max_dist (float): Maximum distance allowed for each path when handling mutli-robot case
        depth (int): Internal parameter used to track re-try recursion depth
        resample (int): Each solution path will be resampled to have
                        `resample` number of points
        start_nodes (ndarray): (# num_vehicles, ndim); Optionl array of start nodes from which 
                                to start each vehicle's solution path
        end_nodes (ndarray): (# num_vehicles, ndim); Optionl array of end nodes at which 
                                to end each vehicle's solution path
        time_limit (int): TSP runtime time limit in seconds

    Returns:
        paths (ndarray): Solution paths
        distances (list): List of path lengths
    """
    if depth > 5:
        print('Warning: Max depth reached')
        return None, None

    # Add the start and end nodes to the node list
    if end_nodes is not None:
        assert end_nodes.shape == (num_vehicles, nodes.shape[-1]), \
            "Incorrect end_nodes shape, should be (num_vehicles, ndim)!"
        nodes = np.concatenate([end_nodes, nodes])
    if start_nodes is not None:
        assert start_nodes.shape == (num_vehicles, nodes.shape[-1]), \
            "Incorrect start_nodes shape, should be (num_vehicles, ndim)!"
        nodes = np.concatenate([start_nodes, nodes])

    # Add dummy 0 location to get arbitrary start and end node sols
    if start_nodes is None or end_nodes is None:
        distance_mat = np.zeros((len(nodes)+1, len(nodes)+1))
        distance_mat[1:, 1:] = pairwise_distances(nodes, nodes)*1e4
        trim_paths = True #shift to account for dummy node
    else:
        distance_mat = pairwise_distances(nodes, nodes)*1e4
        trim_paths = False
    distance_mat = distance_mat.astype(int)
    max_dist = int(max_dist*1e4)

    # Get start and end node indices for ortools
    if start_nodes is None:
        start_idx = np.zeros(num_vehicles, dtype=int)
        num_start_nodes = 0
    else:
        start_idx = np.arange(num_vehicles)+int(trim_paths)
        num_start_nodes = len(start_nodes)

    if end_nodes is None:
        end_idx = np.zeros(num_vehicles, dtype=int)
    else:
        end_idx = np.arange(num_vehicles)+num_start_nodes+int(trim_paths)

    # used by ortools
    def distance_callback(from_index, to_index):
        from_node = manager.IndexToNode(from_index)
        to_node = manager.IndexToNode(to_index)
        return distance_mat[from_node][to_node]

    # num_locations, num vehicles, start, end
    manager = pywrapcp.RoutingIndexManager(len(distance_mat), 
                                           num_vehicles, 
                                           start_idx.tolist(),
                                           end_idx.tolist())
    routing = pywrapcp.RoutingModel(manager)
    transit_callback_index = routing.RegisterTransitCallback(distance_callback)
    routing.SetArcCostEvaluatorOfAllVehicles(transit_callback_index)

    if num_vehicles > 1:
        # Dummy distaance constraint to ensure all paths have similar length
        dimension_name = "Distance"
        routing.AddDimension(
            transit_callback_index,
            0,  # no slack
            max_dist,  # vehicle maximum travel distance
            True,  # start cumul to zero
            dimension_name,
        )
        distance_dimension = routing.GetDimensionOrDie(dimension_name)
        distance_dimension.SetGlobalSpanCostCoefficient(100)

    search_parameters = pywrapcp.DefaultRoutingSearchParameters()
    search_parameters.first_solution_strategy = (
        routing_enums_pb2.FirstSolutionStrategy.PATH_CHEAPEST_ARC
    )
    search_parameters.local_search_metaheuristic = (
        routing_enums_pb2.LocalSearchMetaheuristic.GUIDED_LOCAL_SEARCH
    )
    search_parameters.time_limit.seconds = time_limit
    solution = routing.SolveWithParameters(search_parameters)

    paths = None
    if solution is not None:
        paths, distances = get_routes(manager, routing, 
                                      solution, num_vehicles, 
                                      start_idx, end_idx, trim_paths)
        for path in paths:
            if len(path) < 2:
                print('TSP Warning: Empty path detected')
                return run_tsp(nodes, num_vehicles, int(np.mean(distances)*(1.5/depth)), depth+1)
    else:
        print('TSP Warning: No solution found')
        return run_tsp(nodes, num_vehicles, int(max_dist*1.5), depth+1)

    # Map paths from node indices to node locations
    paths = [nodes[path] for path in paths]

    # Resample each solution path to have resample number of points
    if resample is not None:
        paths = np.array([resample_path(path, resample) for path in paths])

    # Convert distances back to floats in the original scale of the nodes
    distances = np.array(distances)/1e4
    return paths, distances