updated kinematics

+ added ik rewrite; check against paper (5/16)