Fixing the controller (+making code match the math)

[Samir Menon]

During dynamic motions, a lower priority task that is closer to the base seems to be perfomed more accurately than a higher priority task that is closer to the leaves.

This is a major problem.

[Samir Menon]

This is probably because the null space projections don't follow Sentis' paper where the Jacobians themselves have a null space multiplied to them.

See pg. 511 here: http://www.me.utexas.edu/~lsentis/files/ijhr-05.pdf

[Samir Menon]

There are also issues with hybrid force-position control with non-orthogonal task spaces and/or contact constraints.

[Samir Menon]

This is the top pri now.

[Samir Menon]

Will be fixed in the second generation of the task controller.

[Samir Menon]

In progress...