mit-acl / mader C++ 629 92 Trajectory Planner in Multi-Agent and Dynamic Environments optimization path-planning trajectory-optimization multi-agent