Robot Collision Aware Planner