Scaffolding for part 1

Now to actually fill in Jacobian Transpose and Pseudo Inverse...
Pseudo Inverse method for part 1
Jacobian Transpose method for part 1

...I was overthinking it.  Sigh.  Stupid theta notation for Jacobians...
Refactor for part 2
Jacobian Transpose method for part 2
More refactoring, to centralize algorithms

Next up, proper composition support for part 3.
Transformation composition for part 3

Now, instead of performing two transformations for each of Position and
Orientation Control and summing the delta_thetas together, compose the
Jacobians and delta vectors, and perform a single transformation with
that.

A nice side-effect is that now Pseudo Inverse works when both Position
and Orientation Control is enabled!  Still need to figure out why it
doesn't work when only Orientation Control is enabled...
State machine for part 4

It's working!  It's working!!!!

Still need to improve some things, but the basic functionality is all
there.  The object could line up over the target a bit better, and
CLOSEGRIP_WAIT could probably converge faster...
State machine cleanup, no more _WAIT steps

Gripping is instantaneous, it seems, so no need to wait for the gripper
to finish gripping!  It was just taking a while because we were waiting
for a double to converge to exactly zero anyways...lucky that it was
actually happening!
Implement a waving animation when complete!

When the state machine finishes, slowly open and close grip (without
touching object) to have something fun at the ending.  Why not, right?
Only part 4 should be rotating the hand

Turns out this was why pseudo inverse was doing weird things for just
Orientation Control (strange that combined Position and Orientation
still worked fine...go figure).  We only want to apply the 90deg
rotation quaternion when in the state machine.
Homework 1 done!

It's not perfect, but it's done!
Merge in completed homework 1

Hooray!  Due date was revised to the 15th.