Smoothing

Implemented a simple proportional controller (started from cobra_demo.launch)