Fixed logging and moveit issues

Lots of convenience enhancements and bugfixes.
This compiles fine and works for me :).