Refactor of the NAOKinematics class for H21, H25 is comming soon

Signed-off-by: eldr4d <nikofinas@gmail.com>