Fix bug in Orientation, modify RelativePosition, add a test for jacobian

Fix bug in Orientation::computeError ()

Make mask of RelativePosition in joint1 frame instead of world frame

Add a test for jacobian.