Coverage for src/pyroboplan/planning/graph_search.py: 92%

1"""Implementations of graph search algorithms for planning applications.""" 

2 

3import numpy as np 

4 

5from heapq import heappop, heappush 

6 

7from pyroboplan.core.utils import configuration_distance 

8from pyroboplan.planning.utils import retrace_path 

9 

10 

11def dfs(graph, start_node, goal_node): 

12 """ 

13 Find a path between the `start_pose` and `goal_pose` using a depth first search (DFS). 

14 

15 Both the start and goal poses must be present in the Graph. 

16 

17 Parameters 

18 ---------- 

19 graph : `pyroboplan.planning.graph.Graph` 

20 The graph containing the nodes and edges. 

21 start_pose : `pyroboplan.planning.graph.Node` 

22 The starting robot configuration. 

23 goal_pose : `pyroboplan.planning.graph.Node` 

24 The goal robot configuration. 

25 

26 Returns 

27 ------- 

28 path : list of `pyroboplan.planning.graph.Node` 

29 The sequence of nodes representing a path from start_node to goal_node. 

30 """ 

31 # Ensure the requested configurations are present 

32 start_node = graph.get_node(start_node.q) 

33 goal_node = graph.get_node(goal_node.q) 

34 

35 # Mark the start node as the beginning 

36 stack = [start_node] 

37 start_node.parent = None 

38 start_node.cost = 0.0 

39 

40 visited = set() 

41 while stack: 41 ↛ 57line 41 didn't jump to line 57 because the condition on line 41 was always true

42 current = stack.pop() 

43 if current == goal_node: 

44 return retrace_path(current) 

45 

46 if current not in visited: 46 ↛ 41line 46 didn't jump to line 41 because the condition on line 46 was always true

47 visited.add(current) 

48 for neighbor in current.neighbors: 

49 if neighbor not in visited: 

50 # Update the path and mark it to visit 

51 neighbor.parent = current 

52 neighbor.cost = current.cost + current.neighbors[neighbor] 

53 stack.append(neighbor) 

54 

55 # If we reach this point, then we have visited all nodes that are reachable from the start 

56 # node without finding a path, so no path exists. 

57 return None 

58 

59 

60def astar( 

61 graph, 

62 start_node, 

63 goal_node, 

64 heuristic=lambda n1, n2: configuration_distance(n1.q, n2.q), 

65): 

66 """ 

67 Finds the shortest path between the start_pose and goal_pose using the A* algorithm. 

68 

69 Both the start and goal poses must be present in the Graph. 

70 

71 Parameters 

72 ---------- 

73 graph : `pyroboplan.planning.graph.Graph` 

74 The graph containing the nodes and edges. 

75 start_pose : `pyroboplan.planning.graph.Node` 

76 The starting robot configuration. 

77 goal_pose : `pyroboplan.planning.graph.Node` 

78 The goal robot configuration. 

79 heuristic : function(`pyroboplan.planning.graph.Node`, `pyroboplan.planning.graph.Node`) 

80 Heuristic function for estimating the distance between two nodes. For A* this will be 

81 used to compute the configuration space distance between a visited node (arg1) and the 

82 goal node (arg2). Defaults to :func:`pyroboplan.core.utils.configuration_distance`. 

83 

84 Returns 

85 ------- 

86 path : list of `pyroboplan.planning.graph.Node` 

87 The sequence of nodes representing a path from start_node to goal_node. 

88 """ 

89 # Ensure the requested configurations are present 

90 start_node = graph.get_node(start_node.q) 

91 goal_node = graph.get_node(goal_node.q) 

92 

93 # Store nodes as a tuple with cost as the first element, heapq will automatically 

94 # sort based on the first element. We use a counter as the second element to avoid 

95 # having to rely on node values for tie-breaking in pushes. 

96 open_set_nodes = set() # For faster element lookup 

97 open_set = [] 

98 counter = 0 

99 heappush(open_set, (0, counter, start_node)) 

100 open_set_nodes.add(start_node) 

101 

102 # Initialize cost dictionaries 

103 g_scores = {start_node: 0} 

104 f_scores = {start_node: heuristic(start_node, goal_node)} 

105 

106 while open_set: 106 ↛ 135line 106 didn't jump to line 135 because the condition on line 106 was always true

107 # Grab the estimated best node 

108 _, _, current = heappop(open_set) 

109 open_set_nodes.remove(current) 

110 

111 # Then we're done 

112 if current == goal_node: 

113 return retrace_path(goal_node) 

114 

115 for neighbor in current.neighbors: 

116 # If we haven't visited then the assumed cost is infinity 

117 if neighbor not in g_scores: 

118 g_scores[neighbor] = np.inf 

119 

120 # Check is this is a better parent 

121 tentative_g_score = g_scores[current] + current.neighbors[neighbor] 

122 if tentative_g_score < g_scores[neighbor]: 

123 neighbor.parent = current 

124 g_scores[neighbor] = tentative_g_score 

125 neighbor.cost = tentative_g_score 

126 f_score = tentative_g_score + heuristic(neighbor, goal_node) 

127 f_scores[neighbor] = f_score 

128 counter += 1 

129 if neighbor not in open_set_nodes: 129 ↛ 115line 129 didn't jump to line 115 because the condition on line 129 was always true

130 open_set_nodes.add(neighbor) 

131 heappush(open_set, (f_score, counter, neighbor)) 

132 

133 # If we reach this point, then we have visited all nodes that are reachable from the start 

134 # node without finding a path, so no path exists. 

135 return None