Changed gradient in quat2rotmat to what it should be if normalization is enabled and considered a fundamental part of the function. This fixes testForwardKin and testGeometricJacobian, but I'm not sure yet whether it's the way to go.
Now computing twists, dtwistsdq based on J, dJdq.
Now reusing dcrm, dcrf code.
Got rid of twistAdjoint, dTwistAdjoint (duplicated from crm, crf, dcrm, dcrf).
Now using chain rule to compute quat2rotmat gradient.
Profiled dHdq, now using sparse.
Fixed tiny bug in test.
Finally found where I was missing the gradient of a normalization (in doKinematics). Quick fix for now, but testDoKinematics finally passes for non-unit quaternions.