Coverage for src/pyroboplan/planning/graph.py: 95%
130 statements
« prev ^ index » next coverage.py v7.6.12, created at 2025-03-02 22:03 -0500
1"""Generic graph class for use in robot motion planning algorithms."""
3import numpy as np
4import ast
5import re
7from ..core.utils import configuration_distance
10class Node:
11 """Describes an individual node in a graph."""
13 def __init__(self, q, parent=None, cost=0.0):
14 """
15 Creates a graph node instance.
17 Parameters
18 ----------
19 q : array-like
20 The joint configuration associated with the node.
21 parent : `pyroboplan.planning.graph.Node`, optional.
22 The parent of the node, which is either None or another Node object.
23 cost : float, optional
24 The cost associated with the node.
25 """
26 self.q = np.array(q)
27 self.parent = parent
28 self.cost = cost
30 # Dictionary of neighboring nodes and their distances
31 self.neighbors = {}
33 def __hash__(self):
34 """Hash on joint configurations only."""
35 return hash(self.q.tobytes())
37 def __eq__(self, other):
38 """A node is equal to another node if and only if their joint configurations are equal."""
39 return np.array_equal(self.q, other.q)
41 def __lt__(self, other):
42 """Compare nodes based on their lexicographical joint configurations."""
43 return tuple(self.q) < tuple(other.q)
45 def __str__(self):
46 """Return a string representation of the node that includes joint configuration and cost."""
47 return f"Node(q={self.q.tolist()}, cost={self.cost})"
49 @staticmethod
50 def parse(s):
51 """Reconstruct a Node object from its string representation."""
52 pattern = r"Node\(q=(.*?), cost=(.*?)\)"
53 match = re.match(pattern, s)
54 if match: 54 ↛ 58line 54 didn't jump to line 58 because the condition on line 54 was always true
55 q = ast.literal_eval(match.group(1))
56 cost = float(match.group(2))
57 return Node(q, None, cost)
58 raise ValueError(f"Failed to reconstruct node from string {s}.")
61class Edge:
62 """Describes an individual edge in a graph."""
64 def __init__(self, nodeA, nodeB, cost=0.0):
65 """
66 Creates a graph edge instance.
68 Parameters
69 ----------
70 nodeA : `pyroboplan.planning.graph.Node`
71 The first node in the edge.
72 nodeB : `pyroboplan.planning.graph.Node`
73 The second node in the edge.
74 cost : float, optional
75 The cost associated with the edge.
76 """
77 self.nodeA = nodeA
78 self.nodeB = nodeB
79 self.cost = cost
81 def __hash__(self):
82 """Computes the hash of this edge using its nodes' hashes."""
83 return hash((self.nodeA, self.nodeB))
85 def __eq__(self, other):
86 """Two edges are equal if and only if they both start and end at the same nodes."""
87 return self.nodeA == other.nodeA and self.nodeB == other.nodeB
89 def __str__(self):
90 """Return a string representation of the edge."""
91 return f"Edge(nodeA=({self.nodeA}), nodeB=({self.nodeB}), cost={self.cost})"
93 @staticmethod
94 def parse(s):
95 """Reconstruct an Edge object from its string representation."""
96 pattern = r"Edge\(nodeA=\((.*?)\), nodeB=\((.*?)\), cost=(.*?)\)"
97 match = re.match(pattern, s)
98 if match: 98 ↛ 103line 98 didn't jump to line 103 because the condition on line 98 was always true
99 nodeA = Node.parse(match.group(1))
100 nodeB = Node.parse(match.group(2))
101 cost = float(match.group(3))
102 return Edge(nodeA, nodeB, cost)
103 raise ValueError(f"Failed to reconstruct edge from string {s}.")
106class Graph:
107 """Describes a graph of robot configuration states for motion planning."""
109 def __init__(self):
110 """
111 Creates a graph instance with no edges or nodes.
112 """
113 self.nodes = {}
114 self.edges = set()
116 def add_node(self, node):
117 """
118 Adds a node to the graph.
120 Parameters
121 ----------
122 node : `pyroboplan.planning.graph.Node`
123 The node to add to the graph.
124 """
125 if node in self.nodes:
126 return
128 self.nodes[node] = node
130 def get_node(self, q):
131 """
132 Returns the node corresponding to the provided robot configuration, or None if not present.
134 Raises a `ValueError` if the specified configuration is not in the Graph.
136 Parameters
137 ----------
138 q : array-like
139 The robot configuration for the desired node.
141 Returns
142 -------
143 `pyroboplan.planning.graph.Node`
144 The node with the specified robot configuration.
145 """
146 node = Node(q)
147 if node not in self.nodes:
148 raise ValueError("Specified robot configuration not found in Graph.")
150 return self.nodes[node]
152 def remove_node(self, node):
153 """
154 Removes a node from the graph, along with all of its corresponding edges.
156 Parameters
157 ----------
158 node : `pyroboplan.planning.graph.Node`
159 The node to remove from the graph.
161 Returns
162 -------
163 bool :
164 True if the graph was modified, False otherwise.
165 """
166 if node not in self.nodes:
167 return False
169 neighbors = list(self.nodes[node].neighbors)
170 for neighbor in neighbors:
171 self.remove_edge(node, neighbor)
173 del self.nodes[node]
174 return True
176 def add_edge(self, nodeA, nodeB):
177 """
178 Adds an edge to the graph.
180 This is an undirected graph, so if A -> B is an edge, so is B -> A.
182 Parameters
183 ----------
184 nodeA : `pyroboplan.planning.graph.Node`
185 The first node in the edge.
186 nodeB : `pyroboplan.planning.graph.Node`
187 The second node in the edge.
189 Returns
190 -------
191 `pyroboplan.planning.graph.Edge`
192 The edge that was added.
193 """
194 if nodeA not in self.nodes or nodeB not in self.nodes: 194 ↛ 195line 194 didn't jump to line 195 because the condition on line 194 was never true
195 raise ValueError("Specified nodes are not in Graph, cannot add edge.")
197 # Always insert with the "smaller" node as nodeA
198 if nodeA > nodeB:
199 nodeA, nodeB = nodeB, nodeA
201 nodeA = self.nodes[nodeA]
202 nodeB = self.nodes[nodeB]
204 cost = configuration_distance(nodeA.q, nodeB.q)
205 edge = Edge(nodeA, nodeB, cost)
206 self.edges.add(edge)
207 nodeA.neighbors[nodeB] = cost
208 nodeB.neighbors[nodeA] = cost
209 return edge
211 def remove_edge(self, nodeA, nodeB):
212 """
213 Attempts to remove an edge from a graph, if it exists.
215 Parameters
216 ----------
217 nodeA : `pyroboplan.planning.graph.Node`
218 The first node in the edge.
219 nodeB : `pyroboplan.planning.graph.Node`
220 The second node in the edge.
222 Returns
223 -------
224 bool
225 True if the edge was successfully removed, else False.
227 """
228 # Recall that nodes are always inserted with the "smaller" node as nodeA
229 if nodeA > nodeB:
230 nodeA, nodeB = nodeB, nodeA
232 edge = Edge(nodeA, nodeB)
233 if edge not in self.edges:
234 return False
236 del self.nodes[edge.nodeA].neighbors[edge.nodeB]
237 del self.nodes[edge.nodeB].neighbors[edge.nodeA]
238 self.edges.remove(edge)
239 return True
241 def get_nearest_node(self, q):
242 """
243 Gets the nearest node to a specified robot configuration.
244 If the configuration is in the graph the corresponding node will be returned. Namely,
245 the node with distance 0.
247 Parameters
248 ----------
249 q : array-like
250 The robot configuration to use in this query.
252 Returns
253 -------
254 `pyroboplan.planning.graph.Node` or None
255 The nearest node to the input configuration, or None if the graph is empty.
256 """
257 nearest_node = None
258 min_dist = np.inf
259 chk_node = Node(q)
260 if chk_node in self.nodes: 260 ↛ 261line 260 didn't jump to line 261 because the condition on line 260 was never true
261 return self.nodes[chk_node]
263 for node in self.nodes:
264 dist = configuration_distance(q, node.q)
265 if dist < min_dist:
266 min_dist = dist
267 nearest_node = node
269 return nearest_node
271 def get_nearest_neighbors(self, q, radius):
272 """
273 Gets a list of the nearest neighbors to the specified robot configuration.
275 Neighboring nodes will be returned as a sorted list of tuples of nodes along with the distance
276 to the specified configuration. If the provided configuration is in the graph, it will not
277 be returned (no distance 0 node will be returned).
279 Parameters
280 ----------
281 q : array-like
282 The robot configuration to use in this query.
283 radius: float
284 The maximum radius in which to consider neighbors
286 Returns
287 -------
288 List of (`pyroboplan.planning.graph.Node`, `float`)
289 A list of tuples of all the nodes within the specified radius of the provided robot configuration,
290 along with their corresponding distances.
291 """
292 neighbors = []
293 chk_node = Node(q)
294 for node in self.nodes:
295 if node == chk_node:
296 continue
297 d = configuration_distance(node.q, chk_node.q)
298 if d <= radius:
299 neighbors.append((node, d))
300 # Sort based on the second entry in the tuples, namely the distance from q.
301 return sorted(neighbors, key=lambda n: n[1])
303 def save_to_file(self, filename):
304 """
305 Write the graph to file.
307 Only node and edge data is persisted. Neighbors can be rederived, but all parent information is lost.
309 Parameters
310 ----------
311 filename : str
312 The full filepath in which to save the Graph.
313 """
314 with open(filename, "w") as file:
315 file.writelines([str(node) + "\n" for node in self.nodes])
316 file.writelines([str(edge) + "\n" for edge in self.edges])
318 @classmethod
319 def load_from_file(cls, filename):
320 """
321 Loads a graph from a file.
323 Parameters
324 ----------
325 filename : str
326 The full filepath from which to read a Graph.
328 Returns
329 -------
330 `pyroboplan.planning.graph.Graph`
331 The reconstructed Graph from file.
332 """
333 g = cls()
334 with open(filename, "r") as file:
335 for line in file:
336 if line.startswith("Node"):
337 node = Node.parse(line.strip())
338 g.add_node(node)
339 elif line.startswith("Edge"): 339 ↛ 335line 339 didn't jump to line 335 because the condition on line 339 was always true
340 edge = Edge.parse(line.strip())
341 g.add_edge(edge.nodeA, edge.nodeB)
342 return g