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

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

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

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.

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.

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)

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.