Fix bug about LockedDof on JointSO3
Quaternion norm
Merge remote-tracking branch 'origin/master'