fix bug on direct kinematic and jacobian computation