[C++][Bug] Avoid confusion between joint and body methods in se3::Model.

  * se3::Model::getBodyId was used to retrieve joint id in parsers.
  * Implement new methods to retrieve joint ids.
  * Fix body methods.