Qr planner

Needed changes
@yvshri correct the debugger so that it works well with qr_planner

The new cost scheme needs to be tested