Continue implementing forward kinematics

  - test-hpp:body-positions-mesh passes with success.