Initial global path truncation now takes into account robot dimensions