Description
Configuration Space and Motion Planning
C0. 2 points. Consider the diagram above. Two square robots A and B operate in a 2-D workspace. The two
robots do not rotate, and each moves on a fixed track, so that its center remains on the solid gray line shown
in the figure. The robots must move so as not to collide with each other. The diagram is to scale, with each
tick denoting a distance of one unit.
(a) Even though there are two robots both moving in a 2-D workspace, we can still use a 2-D configuration
space to represent the above system. Specify what the axes of our configuration space correspond to in
the workspace, what the axis limits are, and what configuration the above diagram depicts.
(b) Draw the configuration space. For each configuration-space obstacle, label the coordinates of the vertices,
and sketch the workspace configuration corresponding to each. Since the workspace is symmetric, only
makes sketches for the left side of the workspace.
1
In the remainder of this section, we will revisit the 2-DOF 2-link rotational planar robot shown above that we
considered in lecture. The arm is attached to a table surface that we are viewing from a top-down perspective.
There are obstacles on the table as shown by the shaded regions. The goal is move the arm from start
configuration (1, purple) to the goal configuration (2, orange), without colliding into any obstacles. Assume
that the illustrated workspace is 10 units wide and 10 units high; the origin of frame {0} is at the bottom-left
corner, with x0 and y0 axes as shown. The first link of the arm is attached to the table at (6.4, 2.5), the origin
of frame {1}. Link 2 is attached to link 1 at (2.1, 0) with respect to frame {1}, the origin of frame {2}. The
configuration of the arm is given by q = (q1, q2), where q1 is the angle between x0 and x1, and q2 is the angle
between x1 and x2. Both joints may rotate between 0 and 2π radians. For example, the start configuration (1,
purple) is qstart = (0.85, 0.9), and the goal configuration (2, orange) is qgoal = (3.05, 0.05).
Download the starter code (ex2.zip). In this file, you will find:
ex2 cspace.m: Main function. Call ex2 cspace() with an appropriate question number
(1–7) to run the code for that question. Do not modify this file! (Feel free to actually make changes, but
check that your code runs with an unmodified copy of ex2 cspace.m.)
C1.m – C7.m: Code stubs that you will have to fill in for the respective questions.
q2poly.m: Code stub that you will have to fill in, helper function for various questions.
plot obstacles.m: Helper function to draw workspace obstacles defined in ex2 cspace.m.
2 CS 4610/5335
C1: Start configuration. C1: Goal configuration.
C1. 2 points. Plot the robot in the workspace, as shown above. The demonstration code in C1.m shows how to
plot the robot at the zero configuration (q = (0, 0)). You will need to make appropriate transformations to
both links’ polygons and their pivot points (frame origins). Consider filling in q2poly.m first, which is a useful
helper function for C1 and future questions. If you provide qstart and qgoal as input, you should get the figures
above.
Useful functions: Read the documentation for polyshape, a MATLAB class for defining 2-D polygons.
C2. 2 points. Convert the problem into configuration space (right diagram on slide shown above) by discretizing
the configuration space, and checking for collisions at each discrete grid point. Using the specified grid for each
axis given in q grid, compute whether the configuration at each point is in collision or not, by intersecting the
links’ 2-D polygon with the obstacles’ 2-D polygons. Assume that the robot is never in collision with itself.
The resulting matrix should look similar to the configuration space diagram shown on the slide.
Useful functions: intersect
Hint: Future questions rely on the output of C2.m, which may take a while to compute. To avoid re-computing
it in future questions, we have provided functionality to save it in your MATLAB workspace, and pass it in on
future calls:
cspace = ex2 cspace(2);
ex2 cspace(3, cspace);
C3: Distance transform from goal configuration. C4: Path from start to goal.
C3. 2 points. Given a specified goal configuration and the configuration-space grid from C2, compute the distance
transform from the grid point nearest to the goal.
C4. 2 points. Using the distance transform from C3, find a path from the specified start configuration’s closest
grid point to the goal’s grid point. Descend the distance transform in a greedy fashion, breaking ties with any
strategy you wish. Diagonal neighbors are allowed.
C5. 1 point. Convert the path in grid point indices, found in C4, into a path in configurations. Remember to
include the actual start and goal configurations. This should trigger a visualization similar to the one shown
on the next page.
3 CS 4610/5335
C5: Trajectory from start to goal. C6: Swept-volume collisions along the path.
C6. [CS 5335 only.] 2 points. Unfortunately, since collisions have only been checked at discrete grid points,
we cannot guarantee that the segments between those grid points are collision-free. In fact, the trajectory we
found in our implementation of C5 contains three collisions, shown in the right above. These collisions can be
detected by considering the swept volume between two configurations. The swept volume can be approximated
by appropriate convex hulls of the robot links’ 2-D polygons. Check if any segments of the trajectory you found
in C5 are in collision, plot the violating swept volumes (similar to right diagram above), and return the number
of collisions. Depending on how you found your trajectory, it may not actually have any such swept-volume
collisions!
Useful functions: convhull
C7. [CS 5335 only.] 1 point. Most of the collisions above were caused by planning a path that was too close to
obstacles. One simple conservative way to avoid such collisions is to pad the obstacles by a small buffer zone.
Pad the obstacles in configuration space by one grid cell (including diagonal neighbors), and verify that the
resulting trajectory does not contain any swept-volume collisions.
C7: More conservative trajectory from start to goal, with no swept-volume collisions.
4 CS 4610/5335
Sampling-based Motion Planning
Download the starter code (ex2.zip). In this file, you will find:
ex2 motion.m: Main function. Call ex2 motion() with an appropriate question number (0–5)
to run the code for that question. Do not modify this file! (Feel free to actually make changes, but check that
your code runs with an unmodified copy of ex2 motion.m.)
M0.m – M5.m: Code stubs that you will have to fill in for the respective questions.
check collision.m, check edge.m: Helper functions to perform collision checking.
In this part of the assignment, you will implement two sampling-based motion planning algorithms, the probabilistic
roadmap (PRM) and the rapidly exploring random tree (RRT). A 4-DOF 2-link arm has been defined, together with
a spherical obstacle. The cylinders depicting the arm’s links should not collide with the obstacle, but the cylinders
located at the joints are just markers visualizing the axes of rotation and do not contribute to potential collisions.
M0. [CS 5335 only.] 2 points. Familiarize yourself with the provided code in ex2 motion.m and the robot that has
been defined. Use the code in M0.m to visualize the robot in various configurations. The code in ex2 motion.m
that calls M0 also checks whether the configuration is within joint limits, and whether the configuration is in
collision.
Look at the code in check collision.m and check edge.m. Explain what each function is doing to check
for collisions, for individual configurations and for configuration-space line segments respectively. Identify and
describe two potential issues in the collision-checking algorithm (but do not try to fix them).
M1. 1 points. Despite the issues you may have identified in M0, we will proceed with the provided collision checking
functions. Given the provided joint limits, generate uniformly distributed samples from configuration space.
The configurations should be within the joint limits, but they can be in collision.
M2. 2 points. Implement the PRM algorithm to construct a roadmap for this environment. Your graph should
contain num samples collision-free configurations within joint limits. For each vertex, consider the nearest
num neighbors neighbors, and add an edge if the segment between them is collision-free. Edges are specified
using the output adjacency matrix, where adjacency(i,j) is the distance between vertices vi and vj if an
edge exists between them, and is 0 otherwise. Note that the adjacency matrix should be symmetric (edge
between vi and vj implies edge between vj and vi), and that each vertex may have ≤ num neighbors neighbors
(if some edges are in collision) or ≥ num neighbors neighbors (vj may be closest to vi
, but there may be other
vertices closer to vj ).
Hint: The next question relies on the output of M2.m, which may take a while to compute. To avoid recomputing it in future questions, we have provided functionality to save it in your MATLAB workspace, and
pass it in on future calls:
[samples, adjacency] = ex2 motion(2);
ex2 motion(3, samples, adjacency);
M3. 2 points. Use the roadmap to find a collision-free path from the start configuration to the goal configuration.
You will need to appropriate points to get “on” and “off” the roadmap from the start and goal respectively.
Useful functions: shortestpath
M4. 2 points. Implement the RRT algorithm to find a collision-free path from the start configuration to the goal
configuration. Choose appropriate hyperparameter values for the step size and frequency of sampling qgoal.
Remember to traverse the constructed tree to recover the actual path.
M5. 2 points. The path found by RRT likely has many unnecessary waypoints and motion segments in it. Smooth
the path returned by the RRT by removing unnecessary waypoints. One way to accomplish this is to check
non-consecutive waypoints in the path to see if they can be connected by a collision-free edge.
M6. 0 points. If M4 and M5 have been implemented correctly, this question should require no further implementation. Watch your algorithm work on a more challenging motion planning problem. Can you make it even
harder?
5 CS 4610/5335
