add foward kinematics function prototype
fix bug in test, update sample