Fixing the urdf example

Merge #153 first.

Working on #146