Week 4 Optional Exercises: Planning
\[ \newcommand{\E}{\mathbb{E}} \newcommand{\Var}{\operatorname{Var}} \newcommand{\R}{\mathbb{R}} \newcommand{\argmin}{\operatorname*{arg\,min}} \newcommand{\argmax}{\operatorname*{arg\,max}} \newcommand{\diag}{\operatorname{diag}} \newcommand{\SO}{\mathrm{SO}} \newcommand{\SE}{\mathrm{SE}} \newcommand{\T}{\top} \]
These exercises are optional and are not graded. They are based on the material from Lecture 4. Try each exercise on your own first, then click on Solution to reveal a worked answer.
Exercises 1 and 2 use the following undirected graph, where the numbers are edge costs:
| Edge | S-A | S-B | A-B | A-C | B-C | B-D | C-D | C-G | D-G |
|---|---|---|---|---|---|---|---|---|---|
| Cost | 1 | 4 | 2 | 5 | 1 | 6 | 2 | 3 | 2 |
Exercise 1 - Dynamic Programming and Dijkstra
(a) Use the dynamic programming recursion \(D(v, v_{dest}) = \min_{u \in N(v)}[d(v,u) + D(u, v_{dest})]\) to compute the cost-to-go from every node to \(G\).
(b) Run Dijkstra’s algorithm from \(S\). Write down the order in which nodes are removed from the priority queue and the cost-to-come of each node at the moment it is removed. What is the shortest path from \(S\) to \(G\)?
(c) Why does Dijkstra’s algorithm require non-negative edge costs? Give a small example where a negative edge makes it return the wrong answer.
(a) Working backwards from \(D(G) = 0\):
- \(D(D) = \min(2 + 0,\; 2 + D(C)) = 2\).
- \(D(C) = \min(3 + 0,\; 2 + 2,\; 1 + D(B),\; 5 + D(A)) = 3\).
- \(D(B) = \min(1 + 3,\; 6 + 2,\; 2 + D(A),\; 4 + D(S)) = 4\).
- \(D(A) = \min(2 + 4,\; 5 + 3,\; 1 + D(S)) = 6\).
- \(D(S) = \min(1 + 6,\; 4 + 4) = 7\).
(Terms that refer to nodes farther from the goal never achieve the minimum here, so the recursion terminates.)
(b)
| Step | Removed | Cost-to-come | Updates |
|---|---|---|---|
| 1 | S | 0 | A = 1, B = 4 |
| 2 | A | 1 | B = 3, C = 6 |
| 3 | B | 3 | C = 4, D = 9 |
| 4 | C | 4 | D = 6, G = 7 |
| 5 | D | 6 | (G stays 7) |
| 6 | G | 7 | done |
Following parent pointers back from \(G\) gives the shortest path \(S \to A \to B \to C \to G\), with cost \(7\), which matches \(D(S)\) from (a).
(c) Dijkstra’s algorithm assumes that once a node is removed from the queue, its cost-to-come is final, because any other path to it must pass through nodes with at least as large a cost-to-come. A negative edge breaks this. For example, with edges \(S \to A\) (cost \(1\)), \(S \to B\) (cost \(2\)), \(B \to A\) (cost \(-2\)), Dijkstra removes \(A\) with cost \(1\) before discovering the path \(S \to B \to A\) with cost \(0\).
Exercise 2 - A* Search and Heuristics
Use the graph above with the heuristic
| Node | S | A | B | C | D | G |
|---|---|---|---|---|---|---|
| \(h\) | 6 | 5 | 4 | 3 | 2 | 0 |
(a) Is \(h\) admissible? Is it monotonic (consistent), i.e. \(h(u) \le d(u, v) + h(v)\) for every edge?
(b) Run A* from \(S\) to \(G\). Write down the order in which nodes are expanded along with their \(f = g + h\) values. Which nodes does A* avoid expanding compared to Dijkstra?
(c) Change \(h(B)\) to \(10\) and run A* again. What path does it return? Explain what went wrong.
(d) On a 4-connected grid with unit edge costs, is the Manhattan distance admissible? What about on an 8-connected grid where diagonal moves cost \(\sqrt{2}\)? Propose an admissible heuristic for the 8-connected case.
(a) Comparing to the true cost-to-go from Exercise 1(a), \((7, 6, 4, 3, 2, 0)\), we have \(h \le D\) for every node, so \(h\) is admissible. Checking every edge in both directions, e.g. \(h(S) = 6 \le 1 + h(A) = 6\), \(h(C) = 3 \le 3 + h(G) = 3\), \(h(B) = 4 \le 1 + h(C) = 4\), and so on, shows that \(h\) is also consistent.
(b)
| Step | Expanded | \(g\) | \(f\) | Queue after expansion (\(f\) values) |
|---|---|---|---|---|
| 1 | S | 0 | 6 | A: 6, B: 8 |
| 2 | A | 1 | 6 | B: 7 (updated), C: 9 |
| 3 | B | 3 | 7 | C: 7 (updated), D: 11 |
| 4 | C | 4 | 7 | G: 7, D: 8 (updated) |
| 5 | G | 7 | 7 | done |
A* returns the same path \(S \to A \to B \to C \to G\) with cost \(7\), but it never expands \(D\), which Dijkstra did. On large grids, the savings are much bigger, because the heuristic steers the search toward the goal.
(c) With \(h(B) = 10\): expanding \(S\) gives \(A\) (\(f = 6\)) and \(B\) (\(f = 14\)). Expanding \(A\) gives \(B\) (\(g = 3\), \(f = 13\)) and \(C\) (\(g = 6\), \(f = 9\)). Expanding \(C\) gives \(G\) (\(g = 9\), \(f = 9\)) and \(D\) (\(g = 8\), \(f = 10\)). A* then expands \(G\) and returns \(S \to A \to C \to G\) with cost \(9\), which is not optimal. The heuristic overestimates the cost-to-go from \(B\) (\(10 > 4\)), so A* wrongly postpones the node that lies on the optimal path.
(d) On a 4-connected grid, every move changes the Manhattan distance by at most \(1\) at a cost of \(1\), so Manhattan distance never overestimates and is admissible. On an 8-connected grid, a diagonal move reduces the Manhattan distance by \(2\) at a cost of only \(\sqrt{2}\), so Manhattan distance can overestimate and is not admissible. Euclidean distance is admissible in both cases. A tighter admissible choice for 8-connected grids is the octile distance \(h = \max(\Delta x, \Delta y) + (\sqrt{2} - 1)\min(\Delta x, \Delta y)\), which is the exact cost on an empty grid.
Exercise 3 - Configuration Space
(a) A circular robot of radius \(0.5\) m moves (translates only) in a 2D world containing the square obstacle \([2, 4] \times [1, 3]\). Describe the corresponding configuration space obstacle, and compute its area.
(b) Why is it useful to plan in configuration space rather than in the workspace?
(c) If the robot is not a disk but a triangle, and it can also rotate, what is the dimension of its configuration space? How does the shape of the configuration space obstacle change with the robot’s orientation?
(d) A robot arm has \(6\) joints. You discretize each joint angle into \(N = 100\) bins. How many nodes does the grid have? What does this suggest about using grid-based planners for this arm?
(a) The configuration space obstacle is the Minkowski sum of the square and a disk of radius \(0.5\): a rounded square spanning \([1.5, 4.5] \times [0.5, 3.5]\), with quarter-circle corners of radius \(0.5\). Its area is the original square (\(4\)), plus four rectangles of size \(2 \times 0.5\) along the edges (\(4\)), plus four quarter circles (\(\pi \cdot 0.5^2 \approx 0.785\)), for a total of about \(8.79\) m\(^2\).
(b) In configuration space the robot becomes a single point, so collision checking reduces to testing whether a point lies inside a (dilated) obstacle, and standard graph search or sampling-based planners apply directly.
(c) The configuration is \((x, y, \theta)\), so the configuration space is 3D. For each fixed orientation \(\theta\), the configuration space obstacle is the Minkowski sum of the obstacle with the robot’s shape (reflected about its reference point) rotated by \(\theta\), so the cross-section changes shape as \(\theta\) varies. This is why explicitly constructing configuration space obstacles is rarely done for robots with high-dimensional states.
(d) \(100^6 = 10^{12}\) nodes, far too many to store or search. This combinatorial explosion is why sampling-based planners such as RRT and PRM are used for high-dimensional systems.
Exercise 4 - One Iteration of RRT
An RRT for an omnidirectional robot in 2D currently has two nodes: the root \((0, 0)\) and its child \((1, 0)\). The steering function moves at most a distance \(\eta = 1\) along the straight line toward the sample. There is one circular obstacle centered at \((2, 1.5)\) with radius \(0.5\).
(a) The sampler returns \(x_{\text{rand}} = (3, 1)\). Which node is nearest? What is the new node \(x_{\text{new}}\)?
(b) Is the edge from the nearest node to \(x_{\text{new}}\) collision-free? Is \(x_{\text{new}}\) added to the tree?
(c) Brute-force nearest neighbor search examines every node. What is the total cost of nearest neighbor queries over \(n\) RRT iterations? What data structure from lecture improves this, and to what?
(d) Lecture said that RRT exhibits a “Voronoi bias”. Explain what this means and why it makes the tree expand quickly into unexplored space.
(e) Why does sampling roll, pitch, and yaw uniformly not give a uniform distribution over 3D rotations?
(a) The distance to \((0, 0)\) is \(\sqrt{10} \approx 3.16\) and to \((1, 0)\) is \(\sqrt{5} \approx 2.24\), so the nearest node is \((1, 0)\). The direction to the sample is \((2, 1)/\sqrt{5} \approx (0.894, 0.447)\), and the sample is farther than \(\eta\), so \(x_{\text{new}} = (1, 0) + 1 \cdot (0.894, 0.447) = (1.894, 0.447)\).
(b) The closest point on the segment from \((1, 0)\) to \((1.894, 0.447)\) to the obstacle center: projecting \((2, 1.5) - (1, 0) = (1, 1.5)\) onto the unit direction gives \(t = 0.894 + 0.671 = 1.565 > 1\), so the closest point is the endpoint \(x_{\text{new}}\). Its distance to the center is \(\|(0.106, -1.053)\| \approx 1.06 > 0.5\), so the edge is collision-free and \(x_{\text{new}}\) is added to the tree with parent \((1, 0)\).
(c) Iteration \(i\) examines \(i\) nodes, so the total is \(\sum_{i=1}^n i = O(n^2)\). A balanced kd-tree answers each query in \(O(\log n)\) (in low dimensions), giving \(O(n\log n)\) total. Locality-sensitive hashing is another option.
(d) Each node “owns” the Voronoi cell of points that are closer to it than to any other node. A uniform sample selects a node with probability proportional to the area of its Voronoi cell. Nodes on the frontier of the tree have large Voronoi cells extending into unexplored space, so they are extended most often, pulling the tree toward unexplored regions.
(e) The map from Euler angles to rotations is not volume-preserving. Near gimbal lock, many different Euler angle triples represent the same rotation, so uniform Euler angles oversample those rotations (the clustering near the “north pole” shown in lecture). Sampling quaternions with the formula from lecture gives a truly uniform distribution.
Exercise 5 - PRM vs. RRT
(a) When is a PRM preferable to an RRT, and vice versa?
(b) In the offline phase, the PRM stores a matrix \(D[i, j]\) of shortest-path distances between all pairs of nodes. How much memory does this take for \(n = 10^4\) nodes stored as 4-byte floats? How is a query between two new endpoints answered?
(c) Both PRMs and RRTs struggle with narrow passages. Explain why, using the probability that a uniform sample lands inside a passage whose area is \(0.1\%\) of the free space.
(a) A PRM is built once and reused, so it is better for many queries in the same static environment (multi-query planning, e.g. a robot arm in a fixed workcell). An RRT grows a tree from a specific start, so it is better for a single query, for changing environments, and for systems with differential constraints, since it only needs to steer forward from existing nodes instead of connecting arbitrary pairs of states.
(b) \(n^2 = 10^8\) entries \(\times\) \(4\) bytes \(= 400\) MB. To answer a query \(A \to B\), connect \(A\) and \(B\) to nearby PRM nodes (found with a nearest-neighbor or range search, e.g. using a kd-tree) via collision-free edges. This adds one row and one column to \(D\). The shortest path is then \(\min_{i, j}[d(A, i) + D[i,j] + d(j, B)]\) over the nodes \(i\) connected to \(A\) and \(j\) connected to \(B\).
(c) A sample lands in the passage with probability \(p = 0.001\), so on average \(1000\) samples are needed per sample in the passage, and several consecutive samples in the passage are needed to connect through it. The probability of not getting even one sample in the passage after \(n\) samples is \((1 - p)^n\), e.g. about \(37\%\) after \(1000\) samples. Most of the computational effort goes into the large open regions.
Exercise 6 - Visibility Graphs and Path Smoothing
(a) Why is the shortest path in a 2D polygonal environment always contained in the visibility graph? What practical problem does lecture point out with this shortest path?
(b) Consider the smoothing cost from lecture. To keep the path attached to the goal, we also include the last segment, \(\|\mathbf{x}_{\text{dest}} - \mathbf{y}_N\|^2\), in the length term. For a path with a single intermediate point, \(\mathbf{x}_{\text{src}} = (0, 0)\), \(\mathbf{x}_1 = (1, 1)\), \(\mathbf{x}_{\text{dest}} = (2, 0)\), the cost is \[ f(\mathbf{y}_1) = \|\mathbf{y}_1 - \mathbf{x}_1\|^2 + \alpha\left(\|\mathbf{y}_1 - \mathbf{x}_{\text{src}}\|^2 + \|\mathbf{x}_{\text{dest}} - \mathbf{y}_1\|^2\right). \] Find the minimizer \(\mathbf{y}_1\) as a function of \(\alpha\). Evaluate it for \(\alpha = 0\), \(\alpha = 1\), and \(\alpha \to \infty\), and interpret the results.
(c) Why might the smoothed path need to be checked for collisions again?
(a) A shortest path in a polygonal environment is made of straight segments that only bend at obstacle vertices (pulling a string tight around obstacles). Each segment connects two mutually visible points, so it is an edge of the visibility graph. The problem is that this path touches obstacle corners and runs along edges, leaving no safety margin, so in practice the obstacles should be dilated (e.g., by the robot’s radius plus a margin) before building the graph.
(b) Setting the gradient to zero: \[ 2(\mathbf{y}_1 - \mathbf{x}_1) + 2\alpha(\mathbf{y}_1 - \mathbf{x}_{\text{src}}) - 2\alpha(\mathbf{x}_{\text{dest}} - \mathbf{y}_1) = 0 \;\Rightarrow\; \mathbf{y}_1 = \frac{\mathbf{x}_1 + \alpha(\mathbf{x}_{\text{src}} + \mathbf{x}_{\text{dest}})}{1 + 2\alpha} = \frac{(1 + 2\alpha,\; 1)}{1 + 2\alpha}. \]
- \(\alpha = 0\): \(\mathbf{y}_1 = (1, 1)\), i.e. the original path is unchanged.
- \(\alpha = 1\): \(\mathbf{y}_1 = (1, 1/3)\), a compromise between the original path and a short path.
- \(\alpha \to \infty\): \(\mathbf{y}_1 \to (1, 0)\), the midpoint of the straight line from start to goal (the shortest path, ignoring the original one).
(c) Smoothing pulls the path away from the planner’s collision-free waypoints toward a straighter line, which can cut corners through obstacles. The smoothed path should be checked for collisions, and smoothing should stop (or be constrained) where it comes too close to obstacles.