Customjointt implementation and generic ID test

[MaartenAfschrift]

This PR handles:

• For loop implementation of a customjoint. The current code loops over the 6 DOFs in a customjoint instead and avoids the code repetition in the old version.
• Added PlanarJoint (this is typcially used in 2D models for the floating base)
• Added generic test for ID. The old version used a hard coded motion file (that only works for specific models) to compare the ID solutions with the ID-tool and the external function. This version creates a dummy motion file that depending on the coordinates in the model.

[antoinefalisse]

@MaartenAfschrift Thanks! It looks, there is just something weird for the ID test, see comment.