test_forward_kinematics function works