Efficient nullspace projection for FSNS and FOSNS

[rethink-forrest]

Pull request #30 fixed the nullspace projection, but was not the most efficient method.

In the SNSsingle function, instead of taking in the higherPriorityNull and returning the nullSpaceProjector, it looks like the correct method would be to take in the higherPriorityZ and return tildeZ. The math in pinv_QR_Z may have to change for secondary tasks. Does that sound the like the correct direction?

[fflacco]

The pinv_QR_Z performs the redundancy resolution, based on reduced QR, described in

O. Kanoun, F. Lamiraux, and P.-B. Wieber, “Kinematic control of redundant manipulators: Generalizing the task-priority framework to inequality task,” IEEE Trans. on Robotics, vol. 27, no. 4, pp. 785–792, 2011.

then we should use the reduced projector *nullSpaceProjector = tildeZ instead of *nullSpaceProjector = tildeZ * tildeZ.transpose() that is not taking advantage of the reduced form.

Pay attention that it uses the relation pinv(J_P)= tildeZ *pinv(J_tildeZ). This is already taken into account inside pinv_QR_Z, but if you need the complete projector P, then you can resort in *nullSpaceProjector = tildeZ * tildeZ.transpose().