Add ddinvT to invHT.
Add dJ support to bodyKin
Add dJ output to drakeFunction.kinematic.RelativePosition
Add bodyKinTest
Add invHTstd and test.

Computes the inverse of a homogeneous transform with derivatives up to second
order. Assumes the standard geval format for derivatives.
Fix comment in invHTstd.
Fix second derivatives in invHT and bodyKin.

bodyKinTest passes!
Modify matGradMultMat to work with TaylorVars

Can't use kron(speye(...)) with TaylorVars because they don't implement "find".
Change output argument order in bodyKin

P should come before J since it's not a gradient w.r.t. q.
Update PlanarRBM/bodyKin for RBM/bodyKin changes
Add second derivative support to RelativePosition
Add second derivative support to Concatenated
Add doc for dJ in RelativePosition
runLQR operating point is now (x_nom,u_nom)

This makes SimpleFourBar/runLQR easier to read, as there aren't multiple
distinct meanings for x0.
Remove commented out lines from RelativePosition.m
Add description of dJ format to bodyKin
Remove invHTstd and test.

These were crucial in getting the second derivatives right for invHT, as they
let me test with geval. We don't use them in practice though, so I'm removing
them from the working tree. They're still in the git history, of course.

Revert "Fix comment in invHTstd."

This reverts commit 5256a6490262b9312652f5b09a80dffe1dac195e.

Revert "Add invHTstd and test."

This reverts commit 48fec36091f96580875158593ca502e8b7829f63.
 Update argument order for calls to bodyKin
Remove erroneous debugging code
Merge pull request #530 from avalenzu/akv-add-second-derivatives-to-drakeFunctions-rebased

Add second derivatives to drakeFunction.Kinematic.RelativePosition (rebased)