Added 6DOF test for PINV and DLS.
Comments: First test for PINV and DLS are working. Jacobian Transpose do not work with inverse orientation...
More tests are required