Dual arm planner with complex constraints