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 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 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 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