Add assert to check quaternion norm. Add Comments in about intergration of SO3 joints