Local interaction-aware motion planning

Summary
Local interaction-aware motion planning