Add computation of Screw transformation matrix

Add computation of transformation matrix in Screw form in
`TransformationMatrix` class.
Add cooresponding unittest code.