RobotLocomotion/drake — pull request #8 by tkoolen (2014-01-29 19:09:04+00:00)
Title: Twist based kinematics

# Changes
* made geometricJacobian work with the RPY and floating base 6-DoF joint parameterization
* now computing twists in doKinematics
* added relativeTwist method, which allows you to compute the twist between any two bodies, expressed in any frame, based on the twists in kinsol
* added spatialAccelerations method to RigidBodyManipulator. Computes derivatives of twists with respect to world
* added geometricJacobianDotV: computes d/dt(J) * v
* implemented analyticalJacobian, which computes the same Jacobians as forwardkin, but using geometricJacobian as an intermediate result
* made findKinematicPath a lot faster

# Stuff I wanted to discuss
* **analyticalJacobian computational requirements**: I compared the computation time for analyticalJacobian to forwardKin. In a simple comparison, forwardKin wins by a long shot because it doesn't have to do much once dTdq has been computed in kinsol. However, if you use analyticalJacobian then you don't need dTdq, so it could possibly be removed from doKinematics if nothing else uses it. The average time required to call doKinematics and compute random Jacobians in Atlas using forwardKin is ~14 ms. The average time required to call a modified version of doKinematics with the dTdq computation removed and compute the same random Jacobians using analyticalJacobian is ~11.5 ms. So the winner depends on whether we can get rid of the dTdq computation. Also, the results may be different depending on how many Jacobians we need to compute after a single call to doKinematics.
* **kinsol contract**: you said that you want doKinematics to just output something that forwardKin can work with. However, I had to write a number of other functions that require kinsol, so there actually is sort of an implicit contract right now. Is that OK?
* **doKinematics computational requirements**: a call to doKinematics for Atlas went from ~17.1 ms before computing twists to ~23.9 ms after. The profiler tells me that the things that take a lot of time unnecessarily are indexing into an object array (the rigid bodies) and recomputing motion subspaces all the time (should be constant except for rpydot/quatdot parameterized 6-DoF joint). Also transforming twists (line 147 of doKinematics) takes more time than what I think should be necessary.