Constrained integration in scl spatial integrator

[Samir Menon]

Going to add a very preliminary implementation (somewhat inaccurate) to the scl_spatial integrator.

This only applies kinematic constraints purely at the integrator. As such, it assumes that there may be arbitrarily large internal forces in the articulated body system. If the controller applies those, however, there will be no motion along the constrained axes.

[Samir Menon]

Also, for now we assume that the constraint space doesn't change rapidly. (To avoid computing the time derivative of the null space).

This should be fixed later.

[Samir Menon]

Finally, we'll assume that someone can provide the constraint Jacobian to the dynamics engine.

[Samir Menon]

Basic commit 1d84187faba1535568c137ce1481b62878fb637d

For now, we'll focus on solving closed loop constraints.

[Samir Menon]

Fixed a bug as of bae3c246529006f6a255c2f5f3832938710081fb