Matrix quaternion tools

Created by: AtDinesh

I imported the spatial part of Pinocchio. Most of the functions we need (q2v, v2q, ...) are defined there.