Comment function SO3JointConfig::integrate
Add assert to check quaternions