ENH: sixDoFRigidBodyMotion. Adding linearSphericalAngularSpring restraint.

Moving maximum iteration FatalErrorIn check after constraint while loop.

Adding currentOrientation function to give access to Q, but safely and
with a more descriptive name for the function.