forward kinematics with offset term works