Fix tree creation

[Giulero]

This pr should fix #17.

A cycle over the joints is added to ensure that a parent link is added to the parent list only if it is already in the link list.

This check allows the current loading of urdfs in which the joints order is not the one in the kinematic tree.

I'm also being less strict with the link with no inertia. Before the links with no inertial field in the urdf were skipped. Now I'm not skipping but adding a zero inertia in the computations.

[Giulero]

Thanks @valentino-razza ! :D

[traversaro]

Now I'm not skipping but adding a zero inertia in the computations.

That can make quite a big performance difference in models with a lot of frames (i.e. link with zero mass), such as iCub. Did you consider have an option for this, or store them but in a different form (in iDynTree we store them as "additional frames", to avoid useless link in the computation).

[Giulero]

That can make quite a big performance difference in models with a lot of frames (i.e. link with zero mass), such as iCub. Did you consider have an option for this, or store them but in a different form (in iDynTree we store them as "additional frames", to avoid useless link in the computation).

You're right @traversaro! I could add an additional condition that allows only child links with inertia in the link list.
I'll use #24 for fixing this.