local changes, mainly for debugging
partial work on motion planner