Extracted twist computation out of doKinematics and put it in its own function so as not to affect doKinematics computation time. Also moved relativeTwist and transformSpatialAcceleration to util.