E91 Assignment 3

Task 1: Update software.

The starter code has been updated with some major changes to RRT code to implement bidirectional search, along with some helper functions to do support computations and collision detection.

The major change for RRT is that some helper functions (implemented by the RRT subclass) now are passed a tree index as well as other parameters. You can now use bidirectional search by calling initialize(initState, goalState) -- and unidirectional search still works if you just call initialize(initState). You should not sample the goal state if you are running a bidirectional search! See the starter programs (e.g. rrtArm.py) for examples on how to use this.

You can get the software from this link (this should be the LAST zipfile update before we move to git).

Task 2: Make a box, a ground plane, and a goal pose.

A ground plane can be represented by a line segment, and a box shape will form the support for our robot to step onto. In my code, it looks like:

self.ground = [point2D(-10,0), point2D(10,0)]
self.box = Shape2D(Box2D(0.25, 0, 0.5, 0.15))

To create the goal pose, you will need first sample from your set of stable poses, and then subsequently perform IK to move the swing foot onto the box. Finally, you will need to verify that the resulting state is valid (stable and collision free).

I added some new helper functions to KinModel to help evaluate static stability. Assume that self.xforms holds a list of the transformations for my mostly-random (except for IK) pose. Also assume that self.supportBodies is a list containing the body corresponding to the left foot. Then the stability checking code is:

self.support = self.model.getSupport(self.xforms, self.supportBodies)
self.com = self.model.computeCOM(self.xforms)
isBalanced = self.support.contains(self.com[0])

For collision checking, we assume that self.ignoreBodies is a list containing bodies corresponding to the two feet of the robot. Then, collision checking is implemented by:

    def collides(self):
        bodyshapes = self.model.transformedBodyShapes(self.xforms)
        for (body,shape) in bodyshapes:
            if not (body in self.ignoreBodies):
                if shape.overlapsLine(self.ground[0], self.ground[1]):
                    return True
                if shape.overlaps(self.box):
                    return True
        return False

After a bunch of rejection sampling, I get a goal pose for my planner:

Task 3: Plan to step onto a box.

Run a bidirectional RRT search to find a sequence of states connecting the initial (all zero angles) pose to the goal pose. Remember to sample from your library of stable poses (100-200 should suffice) instead of generating them online. Also, remember not to add any state to the tree unless it is valid -- statically stable and collision free.

My RRT computes the distances between two states as the maximum change in any joint angle (this corresponds to the infinity norm we discussed in class). A maximum angular rate at each joint of 4 degrees per timestep is enforced. Here's the (not-so-optimal) path produced by my planner:

By the way, if you have gifsicle and/or ImageMagick installed (easily obtatined via MacPorts or apt-get), you can make animated GIFs like the one above from a sequence of images. I used the last command in the linked section on this page, which combines both packages to produce nicely optimized GIFs.

Task 4: Smooth your path.

This is what the path looks like after running 250 iterations of the randomized shortcutting algorithm we discussed in class. The original RRT solution had 75 states, the smoothed path has 48.

Troubleshooting/hints

If things aren't working:

Visualize everything you can!
If your planner is failing, try running it without collision checks, or even stability checks.
Don't forget Wednesday lab help hours.

Some operations (like generating hundreds of stable poses, or planning with RRT's) take a really long time. If the end result of such a long computation can be represented as a big NumPy array of states, you can use the load and save functionality to remember computation in between program runs. For example:

try:
    self.stable = numpy.load('stable.npy')
except:
    self.stable = numpy.zeros((200, 13))
    for i in range(200):
        self.stable[i,:] = self.sampleRandomStablePose()
    numpy.save('stable.npy', self.stable)

What to turn in

Produce a webpage demonstrating your program, including movies of the path before and after smoothing. How did your planner perform? How general is your solution? Can you move the goal for the swing foot? Make the box taller/wider/etc.? What smoothing algorithm did you use, and how did it work?

We will review all of the assignments in class on Monday the 11th. Be prepared to show your webpage and/or demonstrate your software in action.

Back to E91 home page