first, ugly, but working pass of URDF

This code is super ugly but I am pushing in at least a working state instead of the current broken version :penguin: