forward kinematics added