In this assignment, you will implement motion planners for two systems: a planar robot arm (with state given by the angles of the joints), and a steered car (with state given by the x,y location of a point on the robot, and the angle that the robot makes with the horizontal).
The instructions for this assignment are briefer, because by now you should have a sense of the type of questions that are interesting to explore in a report. Although I do not ask specific questions here, a good report, for example, might compare different values of k for a k-PRM, explore different environments for RRT, and discuss.
You will plan motions for 2R, 3R, and 4R planar arms. The base of the arm will be at location 0, 0, and the joint angles are measured counter-clockwise as described in class. For simplicity, each link on the arm will be represented by a line segment.
First, you’ll need to write a simple simulator for the arm that computes the locations of end points of the links for a configuration given by angles θ1…θn. You may wish to take a look at the slides for the robotics lectures if you have forgotten how to do this.
I’d recommend also writing some drawing code at this point, so that you can view configurations. You should separate the drawing code and the code that computes the kinematics, since the kinematics are useful in the motion planning algorithm, and you don’t want to slow the planner down by drawing graphics each time.
For graphics, you can use matplotlib, which you can install with ‘pip install matplotlib’ from your command line. Keep the graphics simple.
There are obstacles in the workspace; you could make them either small discs or small polygons for simplicity. The arm may have overlapping segments; we do not worry about self-collision. (You can imagine that the links are at slightly different z-heights.)
The motion planner needs to be able to detect if at some configuration (given by angles) there is a collision between the arm and the obstacles. You may use the Shapely library to check for collisions between polygons. (You are very much permitted to discuss the details of how to do this on Piazza, look up ideas online, etc. Just cite!)
Test your collision detection using the graphics by setting some configurations, and having the screen turn red for collision configurations.
For the arm robot, you write a Probabilistic Roadmap planner, as described here. You should implement the k-PRM version, in which the local planner only attempts to connect a vertex to its nearest k neighbors.
There are two phases in the planner:
Start by generating the roadmap. To do this, you’ll need a sampling method to create the vertices of the graph in the configuration space, a collision detection method to check if these vertices are legal, and a local planner that can attempt to connect vertices.
You’ll need to also find the k nearest neighbors of a new vertex to connect to with the local planner. A simple (but inefficient) solution is to just loop over all previous vertices, using the local planner to check distances, and then sorting the resulting array.
I prefer to use radians for distances. Notice that the distance between the angle 0 is very close to the angle 6.27, since the circle wraps around. There are two things to be careful of when computing angular distance:
Are the angles you are working with in some reasonable range? If you keep rotating a link by adding a small amount to the angle, you might pass 2π. Be careful that you choose a good convention for angles and stick to it (some choose 0..2π; others use negative angles for the bottom half of the circle).
Consider two points on the unit circle. There are two ways to go on the circle between those two points: clockwise and counter-clockwise. The correct distance is the minimum of the two. Notice that if the distance in one direction is d, then the distance in the other direction is 2π − d.
I suggest writing a nice angular distance function and testing it in isolation, since it will be very useful for both the PRM and the RRT.
You should generate some interesting maps. Perhaps a “forest” of small rectangular or disc-shaped obstacles would be interesting. Show pictures of plans generated for your robot in your report. You do not need to animate anything; a good picture would perhaps show all the configurations on a single figure, illustrating how the arm moves from start to goal configuration.
For the car, your task is to construct a Rapidly exploring Random Tree RRT to plan motions for a car-like robot.
Assume the robot is a point, and the configuration of the robot is (x,y,θ), the location of the center of the disc, and the heading of the robot. Assume that the wheels are arranged like those of the tricycle discussed in lecture.
The controls for the robot are v and ω, the forward velocity of the robot and the angular velocity of the robot. If v = 1 and ω = 0 for some period of time, the robot drives forwards, where “forwards” is defined by the heading of the robot θ. Both x and y will change based on the value of θ.
For the mobile robot, I have provided code
that computes a new configuration after taking one of six actions for a
short period of time: forwards (v = 1, ω = 0), backwards (v = − 1, ω = 0), forwards turning
counter-clockwise (v = 1,
ω = 1), backwards turning
counter-clockwise (v = − 1,
ω = 1), forwards turning
clockwise, backwards turning clockwise). Run and read
planar_display.py to see how to use the simulator. The
easiest thing to do is to create a PlanarTrajectory with the
controls_rs controls for a Reeds-Shepp car, and then use
the config_at_t() method at any time to get the (x,y,θ)
configuration at a particular time. You can see an example by running
and reading planar_trajectory.py.
A few notes about the implementation. First, a planar robot has a
configuration (x,y,θ). However,
planarsim.py internally does most of its calculations using
what are called homogeneous coordinate transform matrices. These
matrices encode a rotation matrix and a translation vector that rotate
and translate from the origin to some frame described by (x,y,θ). You
do not need to use these matrices, since there are functions to compute
simple x, y, θ values
from a matrix.
Read at least one paper on the RRT or one paper on the PRM. Briefly describe the main results of each paper in a previous work section of the report.
Implement a variation of PRM or RRT; for example, toggle-PRM, medial axis PRM, or you are feeling particularly adventurous and have quite a bit of extra time, discrete RRT for multi-robot planning or RRT*. (Either of the last two would be worth 5 points.)
Finding nearest neighbors by looping over all points is expensive. A better solution would use some sort of Approximate-Nearest-Neighbor (ANN) algorithm to find the neighbors in logarithmic or amortized constant time. You might try the nearest neighbor module in scikit.