function r = quaternion_constraint(q, dotq, alpha)
    r = dotq'*dotq + 2*alpha*q'*dotq + (1/2)*alpha^2*(q'*q-1);
end
