Socially aware motion planner in object-rich environment