Tensor Roadmap Solver

Tensor Roadmap

class discopygal.solvers.tensor_solver.tensor_roadmap.Roadmap(scene, robot, nearest_neighbors=None, sampler=None)

A class that represents a 2D roadmap, which has the roadmap graph and the collision detection for that roadmap.

Parameters:

  • scene (Scene) – the scene of the roadmap

  • robot (Robot) – robot for which we built the roadmap

  • nearest_neighbors (NearestNeighbors or None) – a nearest neighbors algorithm. if None then use sklearn implementation

  • sampler (Sampler) – sampling algorithm/method. if None then use uniform sampling

add_edge(p, q)

Get two vertices p, q and try to connect them. If we cannot connect them - return False.

Parameters:
Returns:

True if edge was added

Return type:

bool

add_point(point)

Try to add a point to the roadmap. If point is invalid - return False.

Parameters:

point (Point_2) – point to add

Returns:

True if point was added

Return type:

bool

add_random_point()

Sample a random point and try to add it to the roadmap. If point is inavlid - return False

Returns:

True if point was added

Return type:

bool

get_point_in_roadmap(point: Point_2)

Returns the actual point from the roadmap (if object point exists in the roadmap returns it) If the object point doesn’t exist, it returns the object point from the roadmap that has the same coordinates as the given point

Parameters:

point (Point_2) – The point to get from the roadmap. point must already exist on the roadmap!

Returns:

The object point from the roadmap that has same coordinates as the given point

Return type:

Point_2

get_points()

Return a list of all landmarks (points) in the roadmap

Returns:

list of landmarks

Return type:

list<Point_2>

class discopygal.solvers.tensor_solver.tensor_roadmap.TensorRoadmap(roadmaps, scene, nearest_neighbors=None, metric=None)

A class that represents a tensor roadmap, which couples a roadmap of each robot (which was built seperatly) into one large graph product representing motion of all the robots. Note that since for most (nontrivial) cases the explicit roadmap is intractable, we generate an interface which allows us to query the tensor roadmap without explicitly building it - and also remembering the subgraph of queries we made.

Graphs are reprsented using networkx. Vertices of a roadmap of a single robot are Point_2, while vertices of the tensor roadmap are of type Point_d.

Parameters:

  • roadmaps (dict<Robot, networkx.Graph>) – a mapping between each robot and its roadmap

  • nearest_neighbors (NearestNeighbors or None) – a nearest neighbors algorithm. if None then use sklearn implementation

  • metric (Metric or None) – a metric for choosing best edge, can be different then the nearest_neighbors metric! If None then use euclidean metric

add_tensor_vertex(tensor_point, cost=inf)

Add explictly a tensor vector to the graph (for example when adding start and end points)

Parameters:

tensor_point (Point_d) – a point in the high dimensional space

clean_tensor_edge(p, q)

Get a tensor edge (made of tensorr vertices p and q) and clean it, i.e. make sure that no two robots are intersecting each other. If two robots intersect - then make sure one of them stays in place.

Parameters:
Returns:

q’, a vertex for which (p,q’) is a valid motion

Return type:

Point_d

connect_to_roadmap(new_vertex, prev_vertex)

Disconnects previous connection

find_best_edge(tensor_vertex, tensor_point)

Given a vertex in the tensor graph and a random tensor point, choose an edge in the tensor graph that is made of the edges in the original roadmaps that take us the closest to the tensor point.

Also note that if two robots collide, we try to make one of them to stay in place (i.e. some robots might stay in place, but at least one robot is guaranteed to move - if an edge was added).

Returns:

True if an edge was added

Return type:

bool

get_tensor_subgraph()

Get the subgraph of the tensor roadmap discovered so far

nearest_tensor_vertex(tensor_point)

Get a tensor point and return a tensor vertex (i.e. vertex of hat(V), the vertices of the tensor graph) that is the closest when comparing in the high dimensional space.

Parameters:

tensor_point (Point_d) – a point in the high dimensional space

try_connecting(vertex1, vertex2)

Try connecting two vertices of the tensor graph. We do that by the method proposed by de Berg: http://www.roboticsproceedings.org/rss05/p18.html (Finding each path individually and then trying to find a prioritization which is collision free). Return True if we succeeded connecting.

Parameters:

  • vertex1 (Point_d) – first vertex in the tensor roadmap

  • vertex2 – second vertex in the tensor roadmap

Returns:

True if we connected the vertices

Return type:

bool

discopygal.solvers.tensor_solver.tensor_roadmap.build_probabilistic_roadmap(scene, robot, num_landmarks, k, nearest_neighbors=None, sampler=None)

Build a probabilistic roadmap for the robot in a given scene.

Parameters:

  • scene (Scene) – scene

  • robot (Robot) – robot

  • num_landmarks (int) – number of landmarks to sample for the PRM

  • k (int) – number of nearest neighbors to conenct when building PRM

  • nearest_neighbors (NearestNeighbors or None) – a nearest neighbors algorithm. if None then use sklearn implementation

  • sampler (Sampler) – sampling algorithm/method. if None then use uniform sampling

Returns:

roadmap for the given robot

Type:

Roadmap

Tensor Solver

class discopygal.solvers.tensor_solver.tensor_solver.TensorSolver(**kwargs)

Bases: SamplingSolver

Base solver class for solvers that use a tensor roadmap - constructs a two-dimensional roadmap for each robot and searches on the tensor product of these roadmaps.

build_roadmap()

Constructs the roadmap of points in the configuration space which a path will be searched on to find a solution. Every sampling solver should implement how to build the roadmap.

Returns:

The built roadmap. Each node represents a point in configuration space (dimension = 2*robots_num)

Return type:

nx.Graph

abstract build_robot_roadmap(robot)

Creates a two-dimensional roadmap for the given robot

Parameters:

robot (Robot) – The robot o build a roadmap for

Returns:

The roadmap

Return type:

Roadmap

search_path_on_roadmap()

After already constructed a roadmap, plan a path for each robot (i.e. return paths for all the robots) If failed to find a path for each robot return an empty PathCollection (PathCollection())

The search method of the basic class SamplingSolver is simply finding the shortest path according to the weights of the edges (the weight attribute should be called weight)

Returns:

Path collection of motion planning (path for each robot)

Return type:

PathCollection

abstract search_tensor_roadmap()

Search method the explore the tensor roadmap a find a solution path on it