Urdf rigid body manipulator error

Solves Issue #33