I'm trying to implement a hinge joint between two rigid bodies based on Lagnange multipliers, s.t. constraint functions to be constraints on quaternions, which describe bodies orientation, itself. It's more natural than describe constraints in terms of some vectors rigidly connected with bodies as it is just implicit quaternions. Plus quaternions should handle the problem of bistability of "vectorial constraints". It is essential to write the correct constraint functions.
It seems that I found the correct constraints but something goes wrong (later). Tell me, please, if I do something wrong or miss something. Let us concentrate just on unlimited hinge rotation of two rigid bodies about some common axis. So we need to get rid of two degrees of freedom. Let
q = rAconj qAconj qB rB,
where qA and qB are usual orientation quaternions (s.t. if a vector V has a coordinates column V' in a frame of a body B (lets name this frame B') then (qB*(0, V')*qBconj).im is its coordinates column in a world frame (lets call this frame W)), rB is a quaternion which carry out rotation from frame where the axis, bodies rotate about, direction vector has coordinates (0, 0, 1) (lets call this frame B'') to frame B', rA defined by analogy.
The hinge constraint would not be violated iff vectorial part of q is collinear with Z axis. I.e. the (coordinate) constraints are just
q.x = 0,
q.y = 0.
Then differentiating this equations, minding Leibniz rule, unitarity of quaternions and dq/dt = 0.5 q (0, w) identity we get
qdot = 0.5 rAconj (qAconj qB (0, wB) - (0, wA) qAconj qB) rB
and the velocity constraints:
qdot.x = 0,
qdot.y = 0.
Velocities are "decoupled" so to get Jacobian is a matter of simple quaternion multiplication. For convenience while experimenting I just write short functions getC, getCdot by which Jacobian is calculated. I'm quite sure in that part of code 'cause I've tested with "not quaternion version" of getCdot and it works fine.
Everything works fine if one of the bodies has an infinite inertia (kinematic body) or (if both bodies are dynamic) if angular velocities are orthogonal or collinear to rotation axis. If angular velocities are in general case simulation blows up. Can you get a tip why is this happening? This is not because of quadratic term (Coriolis', gyroscopic, ...) in the Euler equations -- I ignore this term for now for the sake of stability.
