Urdf parser

[capobiag]

URDF Parser for cardillo

[capobiag]

Still work in progress. The double pendulum works well. Other examples contain so many special cases that I still need some time.

[capobiag]

The actual version can treat any URDF defining kinematic trees for which the only massless bodies are leaves that are rigidly connected to its parent.

To do's:

• Add prismatic joint
• Add visual for:
  - [x] Mesh
  - [x] Sphere
  - [x] Box
  - [ ] Cylinder
  - [ ] Capsule
• Test with more examples
• Process "fixed" joints by combining the rigid bodies into a single body -> speed-up and no problem with massless bodies.
• clean up code.