Add interface for joint transformation and its derivatives w.r.t. each dof