fix a bug in doKinematics

fix #579