Forward and Inverse Kinematics for the NAO robot in c++