Fix bug.
Fixed spatial velocity computation.
Fixed jacobian computation.
Clean code.
Clean code.
Move code after read macro declaration.

TODO : move code into test.
Clean code.