WIP: Fix/obstacle avoiding controller behavior

Issues to be fixed:

• Overshooting when reaching target pose (likely implemented)
• Movement into opposed / arbitrary direction (based on previous movement) (implemented)