feedback linearization and point offset controller working, implemented the multi-robot strategy too