Optimize control

some optimizations in conjunction with the commits from cob_robots